From 2ae7cc4a94c1ab1dfc0264367c5da4716ea02bfe Mon Sep 17 00:00:00 2001 From: Abdullah Tahiri Date: Thu, 18 May 2023 08:35:55 +0200 Subject: [PATCH] Sketcher: PlaneGCS C-lang formatting ==================================== As per: https://forum.freecad.org/viewtopic.php?t=77205 Please use pre-commit as described in: https://github.com/FreeCAD/DevelopersHandbook/blob/master/gettingstarted/index.md#setting-up-for-development --- src/Mod/Sketcher/App/planegcs/Constraints.cpp | 289 +- src/Mod/Sketcher/App/planegcs/Constraints.h | 1828 +++++++---- src/Mod/Sketcher/App/planegcs/GCS.cpp | 2811 ++++++++--------- src/Mod/Sketcher/App/planegcs/GCS.h | 765 +++-- src/Mod/Sketcher/App/planegcs/Geo.cpp | 565 ++-- src/Mod/Sketcher/App/planegcs/Geo.h | 689 ++-- src/Mod/Sketcher/App/planegcs/SubSystem.cpp | 157 +- src/Mod/Sketcher/App/planegcs/SubSystem.h | 109 +- src/Mod/Sketcher/App/planegcs/Util.h | 20 +- src/Mod/Sketcher/App/planegcs/qp_eq.cpp | 22 +- src/Mod/Sketcher/App/planegcs/qp_eq.h | 4 +- 11 files changed, 3939 insertions(+), 3320 deletions(-) diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index 76f212d33b..b56e19d1c7 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -23,7 +23,7 @@ #include #define DEBUG_DERIVS 0 #if DEBUG_DERIVS -# include +#include #endif #include @@ -63,7 +63,7 @@ void Constraint::redirectParams(const MAP_pD_pD& redirectionmap) void Constraint::revertParams() { pvec = origpvec; - pvecChangedFlag=true; + pvecChangedFlag = true; } ConstraintType Constraint::getTypeId() @@ -81,12 +81,12 @@ double Constraint::error() return 0.0; } -double Constraint::grad(double * /*param*/) +double Constraint::grad(double* /*param*/) { return 0.0; } -double Constraint::maxStep(MAP_pD_D & /*dir*/, double lim) +double Constraint::maxStep(MAP_pD_D& /*dir*/, double lim) { return lim; } @@ -106,7 +106,7 @@ int Constraint::findParamInPvec(double* param) // -------------------------------------------------------- // Equal -ConstraintEqual::ConstraintEqual(double *p1, double *p2, double p1p2ratio) +ConstraintEqual::ConstraintEqual(double* p1, double* p2, double p1p2ratio) { ratio = p1p2ratio; pvec.push_back(p1); @@ -127,14 +127,16 @@ void ConstraintEqual::rescale(double coef) double ConstraintEqual::error() { - return scale * (*param1() - ratio *(*param2())); + return scale * (*param1() - ratio * (*param2())); } -double ConstraintEqual::grad(double *param) +double ConstraintEqual::grad(double* param) { - double deriv=0.; - if (param == param1()) deriv += 1; - if (param == param2()) deriv += -1; + double deriv = 0.; + if (param == param1()) + deriv += 1; + if (param == param2()) + deriv += -1; return scale * deriv; } @@ -248,9 +250,9 @@ double ConstraintCenterOfGravity::error() return scale * (*thecenter() - sum); } -double ConstraintCenterOfGravity::grad(double *param) +double ConstraintCenterOfGravity::grad(double* param) { - double deriv=0.; + double deriv = 0.; if (param == thecenter()) deriv = 1; @@ -603,7 +605,7 @@ double ConstraintPointOnBSpline::grad(double* gcsparam) } // Difference -ConstraintDifference::ConstraintDifference(double *p1, double *p2, double *d) +ConstraintDifference::ConstraintDifference(double* p1, double* p2, double* d) { pvec.push_back(p1); pvec.push_back(p2); @@ -642,7 +644,7 @@ double ConstraintDifference::grad(double* param) // -------------------------------------------------------- // P2PDistance -ConstraintP2PDistance::ConstraintP2PDistance(Point &p1, Point &p2, double *d) +ConstraintP2PDistance::ConstraintP2PDistance(Point& p1, Point& p2, double* d) { pvec.push_back(p1.x); pvec.push_back(p1.y); @@ -732,8 +734,8 @@ double ConstraintP2PDistance::maxStep(MAP_pD_D& dir, double lim) // -------------------------------------------------------- // P2PAngle -ConstraintP2PAngle::ConstraintP2PAngle(Point &p1, Point &p2, double *a, double da_) -: da(da_) +ConstraintP2PAngle::ConstraintP2PAngle(Point& p1, Point& p2, double* a, double da_) + : da(da_) { pvec.push_back(p1.x); pvec.push_back(p1.y); @@ -810,7 +812,7 @@ double ConstraintP2PAngle::maxStep(MAP_pD_D& dir, double lim) // -------------------------------------------------------- // P2LDistance -ConstraintP2LDistance::ConstraintP2LDistance(Point &p, Line &l, double *d) +ConstraintP2LDistance::ConstraintP2LDistance(Point& p, Line& l, double* d) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -840,7 +842,7 @@ double ConstraintP2LDistance::error() double dist = *distance(); double dx = x2 - x1; double dy = y2 - y1; - double d = sqrt(dx * dx + dy * dy); // line length + 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) @@ -932,7 +934,7 @@ double ConstraintP2LDistance::maxStep(MAP_pD_D& dir, double lim) // -------------------------------------------------------- // PointOnLine -ConstraintPointOnLine::ConstraintPointOnLine(Point &p, Line &l) +ConstraintPointOnLine::ConstraintPointOnLine(Point& p, Line& l) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -944,7 +946,7 @@ ConstraintPointOnLine::ConstraintPointOnLine(Point &p, Line &l) rescale(); } -ConstraintPointOnLine::ConstraintPointOnLine(Point &p, Point &lp1, Point &lp2) +ConstraintPointOnLine::ConstraintPointOnLine(Point& p, Point& lp1, Point& lp2) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -1012,7 +1014,7 @@ double ConstraintPointOnLine::grad(double* param) // -------------------------------------------------------- // PointOnPerpBisector -ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point &p, Line &l) +ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point& p, Line& l) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -1024,7 +1026,7 @@ ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point &p, Line &l) rescale(); } -ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point &p, Point &lp1, Point &lp2) +ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point& p, Point& lp1, Point& lp2) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -1075,22 +1077,22 @@ double ConstraintPointOnPerpBisector::error() return scale * err; } -double ConstraintPointOnPerpBisector::grad(double *param) +double ConstraintPointOnPerpBisector::grad(double* param) { - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) return 0.0; double deriv; errorgrad(nullptr, &deriv, param); - return deriv*scale; + return deriv * scale; } // -------------------------------------------------------- // Parallel -ConstraintParallel::ConstraintParallel(Line &l1, Line &l2) +ConstraintParallel::ConstraintParallel(Line& l1, Line& l2) { pvec.push_back(l1.p1.x); pvec.push_back(l1.p1.y); @@ -1154,7 +1156,7 @@ double ConstraintParallel::grad(double* param) // -------------------------------------------------------- // Perpendicular -ConstraintPerpendicular::ConstraintPerpendicular(Line &l1, Line &l2) +ConstraintPerpendicular::ConstraintPerpendicular(Line& l1, Line& l2) { pvec.push_back(l1.p1.x); pvec.push_back(l1.p1.y); @@ -1168,8 +1170,7 @@ ConstraintPerpendicular::ConstraintPerpendicular(Line &l1, Line &l2) rescale(); } -ConstraintPerpendicular::ConstraintPerpendicular(Point &l1p1, Point &l1p2, - Point &l2p1, Point &l2p2) +ConstraintPerpendicular::ConstraintPerpendicular(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2) { pvec.push_back(l1p1.x); pvec.push_back(l1p1.y); @@ -1233,7 +1234,7 @@ double ConstraintPerpendicular::grad(double* param) // -------------------------------------------------------- // L2LAngle -ConstraintL2LAngle::ConstraintL2LAngle(Line &l1, Line &l2, double *a) +ConstraintL2LAngle::ConstraintL2LAngle(Line& l1, Line& l2, double* a) { pvec.push_back(l1.p1.x); pvec.push_back(l1.p1.y); @@ -1248,8 +1249,8 @@ ConstraintL2LAngle::ConstraintL2LAngle(Line &l1, Line &l2, double *a) rescale(); } -ConstraintL2LAngle::ConstraintL2LAngle(Point &l1p1, Point &l1p2, - Point &l2p1, Point &l2p2, double *a) +ConstraintL2LAngle::ConstraintL2LAngle(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, + double* a) { pvec.push_back(l1p1.x); pvec.push_back(l1p1.y); @@ -1347,7 +1348,7 @@ double ConstraintL2LAngle::maxStep(MAP_pD_D& dir, double lim) // -------------------------------------------------------- // MidpointOnLine -ConstraintMidpointOnLine::ConstraintMidpointOnLine(Line &l1, Line &l2) +ConstraintMidpointOnLine::ConstraintMidpointOnLine(Line& l1, Line& l2) { pvec.push_back(l1.p1.x); pvec.push_back(l1.p1.y); @@ -1509,7 +1510,7 @@ double ConstraintTangentCircumf::grad(double* param) // -------------------------------------------------------- // ConstraintPointOnEllipse -ConstraintPointOnEllipse::ConstraintPointOnEllipse(Point &p, Ellipse &e) +ConstraintPointOnEllipse::ConstraintPointOnEllipse(Point& p, Ellipse& e) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -1597,13 +1598,13 @@ double ConstraintPointOnEllipse::grad(double* param) // -------------------------------------------------------- // ConstraintEllipseTangentLine -ConstraintEllipseTangentLine::ConstraintEllipseTangentLine(Line &l, Ellipse &e) +ConstraintEllipseTangentLine::ConstraintEllipseTangentLine(Line& l, Ellipse& e) { this->l = l; this->l.PushOwnParams(pvec); this->e = e; - this->e.PushOwnParams(pvec);//DeepSOIC: hopefully, this won't push arc's parameters + this->e.PushOwnParams(pvec);// DeepSOIC: hopefully, this won't push arc's parameters origpvec = pvec; pvecChangedFlag = true; rescale(); @@ -1611,7 +1612,7 @@ ConstraintEllipseTangentLine::ConstraintEllipseTangentLine(Line &l, Ellipse &e) void ConstraintEllipseTangentLine::ReconstructGeomPointers() { - int i=0; + int i = 0; l.ReconstructOnNewPvec(pvec, i); e.ReconstructOnNewPvec(pvec, i); pvecChangedFlag = false; @@ -1849,8 +1850,10 @@ ConstraintInternalAlignmentPoint2Hyperbola::ConstraintInternalAlignmentPoint2Hyp void ConstraintInternalAlignmentPoint2Hyperbola::ReconstructGeomPointers() { int i = 0; - p.x = pvec[i]; i++; - p.y = pvec[i]; i++; + p.x = pvec[i]; + i++; + p.y = pvec[i]; + i++; e.ReconstructOnNewPvec(pvec, i); pvecChangedFlag = false; } @@ -1982,9 +1985,10 @@ void ConstraintEqualMajorAxesConic::rescale(double coef) scale = coef * 1; } -void ConstraintEqualMajorAxesConic::errorgrad(double *err, double *grad, double *param) +void ConstraintEqualMajorAxesConic::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); double a1, da1; a1 = e1->getRadMaj(param, da1); double a2, da2; @@ -2028,7 +2032,7 @@ ConstraintEqualFocalDistance::ConstraintEqualFocalDistance(ArcOfParabola* a1, Ar void ConstraintEqualFocalDistance::ReconstructGeomPointers() { - int i =0; + int i = 0; e1->ReconstructOnNewPvec(pvec, i); e2->ReconstructOnNewPvec(pvec, i); pvecChangedFlag = false; @@ -2044,9 +2048,10 @@ void ConstraintEqualFocalDistance::rescale(double coef) scale = coef * 1; } -void ConstraintEqualFocalDistance::errorgrad(double *err, double *grad, double *param) +void ConstraintEqualFocalDistance::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); DeriVector2 focus1(this->e1->focus1, param); DeriVector2 vertex1(this->e1->vertex, param); @@ -2075,7 +2080,7 @@ void ConstraintEqualFocalDistance::errorgrad(double *err, double *grad, double * double ConstraintEqualFocalDistance::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; } @@ -2094,7 +2099,7 @@ double ConstraintEqualFocalDistance::grad(double* param) // -------------------------------------------------------- // ConstraintCurveValue -ConstraintCurveValue::ConstraintCurveValue(Point &p, double* pcoord, Curve& crv, double *u) +ConstraintCurveValue::ConstraintCurveValue(Point& p, double* pcoord, Curve& crv, double* u) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -2109,7 +2114,8 @@ ConstraintCurveValue::ConstraintCurveValue(Point &p, double* pcoord, Curve& crv, ConstraintCurveValue::~ConstraintCurveValue() { - delete this->crv; this->crv = nullptr; + delete this->crv; + this->crv = nullptr; } void ConstraintCurveValue::ReconstructGeomPointers() @@ -2135,40 +2141,43 @@ void ConstraintCurveValue::rescale(double coef) scale = coef * 1; } -void ConstraintCurveValue::errorgrad(double *err, double *grad, double *param) +void ConstraintCurveValue::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); double u, du; - u = *(this->u()); du = ( param == this->u() ) ? 1.0 : 0.0; + u = *(this->u()); + du = (param == this->u()) ? 1.0 : 0.0; - DeriVector2 P_to; //point of curve at parameter value of u, in global coordinates - P_to = this->crv->Value(u,du,param); + DeriVector2 P_to;// point of curve at parameter value of u, in global coordinates + P_to = this->crv->Value(u, du, param); - DeriVector2 P_from(this->p, param); //point to be constrained + DeriVector2 P_from(this->p, param);// point to be constrained DeriVector2 err_vec = P_from.subtr(P_to); - if (this->pcoord() == this->p.x){ //this constraint is for X projection + if (this->pcoord() == this->p.x) {// this constraint is for X projection if (err) *err = err_vec.x; if (grad) *grad = err_vec.dx; - } else if (this->pcoord() == this->p.y) {//this constraint is for Y projection + } + else if (this->pcoord() == this->p.y) {// this constraint is for Y projection if (err) *err = err_vec.y; if (grad) *grad = err_vec.dy; - } else { - assert(false/*this constraint is neither X nor Y. Nothing to do..*/); } - + else { + assert(false /*this constraint is neither X nor Y. Nothing to do..*/); + } } double ConstraintCurveValue::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; } @@ -2184,7 +2193,7 @@ double ConstraintCurveValue::grad(double* param) return deriv * scale; } -double ConstraintCurveValue::maxStep(MAP_pD_D &/*dir*/, double lim) +double ConstraintCurveValue::maxStep(MAP_pD_D& /*dir*/, double lim) { // step(angle()) <= pi/18 = 10° /* TODO: curve-dependent parameter change limiting?? @@ -2201,7 +2210,7 @@ double ConstraintCurveValue::maxStep(MAP_pD_D &/*dir*/, double lim) // -------------------------------------------------------- // ConstraintPointOnHyperbola -ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point &p, Hyperbola &e) +ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point& p, Hyperbola& e) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -2214,7 +2223,7 @@ ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point &p, Hyperbola &e) rescale(); } -ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point &p, ArcOfHyperbola &e) +ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point& p, ArcOfHyperbola& e) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -2315,7 +2324,7 @@ double ConstraintPointOnHyperbola::grad(double* param) // -------------------------------------------------------- // ConstraintPointOnParabola -ConstraintPointOnParabola::ConstraintPointOnParabola(Point &p, Parabola &e) +ConstraintPointOnParabola::ConstraintPointOnParabola(Point& p, Parabola& e) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -2326,7 +2335,7 @@ ConstraintPointOnParabola::ConstraintPointOnParabola(Point &p, Parabola &e) rescale(); } -ConstraintPointOnParabola::ConstraintPointOnParabola(Point &p, ArcOfParabola &e) +ConstraintPointOnParabola::ConstraintPointOnParabola(Point& p, ArcOfParabola& e) { pvec.push_back(p.x); pvec.push_back(p.y); @@ -2339,14 +2348,17 @@ ConstraintPointOnParabola::ConstraintPointOnParabola(Point &p, ArcOfParabola &e) ConstraintPointOnParabola::~ConstraintPointOnParabola() { - delete this->parab; this->parab = nullptr; + delete this->parab; + this->parab = nullptr; } void ConstraintPointOnParabola::ReconstructGeomPointers() { - int i=0; - p.x=pvec[i]; i++; - p.y=pvec[i]; i++; + int i = 0; + p.x = pvec[i]; + i++; + p.y = pvec[i]; + i++; this->parab->ReconstructOnNewPvec(pvec, i); pvecChangedFlag = false; } @@ -2361,14 +2373,15 @@ void ConstraintPointOnParabola::rescale(double coef) scale = coef * 1; } -void ConstraintPointOnParabola::errorgrad(double *err, double *grad, double *param) +void ConstraintPointOnParabola::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); DeriVector2 focus(this->parab->focus1, param); DeriVector2 vertex(this->parab->vertex, param); - DeriVector2 point(this->p, param); //point to be constrained to parabola + DeriVector2 point(this->p, param);// point to be constrained to parabola DeriVector2 focalvect = focus.subtr(vertex); @@ -2389,15 +2402,15 @@ void ConstraintPointOnParabola::errorgrad(double *err, double *grad, double *par proj = point_to_focus.scalarProd(xdir, &dproj); if (err) - *err = pf - 2*focal - proj; + *err = pf - 2 * focal - proj; if (grad) - *grad = dpf - 2*dfocal - dproj; + *grad = dpf - 2 * dfocal - dproj; } double ConstraintPointOnParabola::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; } @@ -2405,7 +2418,7 @@ double ConstraintPointOnParabola::grad(double* param) { // first of all, check that we need to compute anything. if (findParamInPvec(param) == -1) - return 0.0; + return 0.0; double deriv; errorgrad(nullptr, &deriv, param); @@ -2432,19 +2445,23 @@ ConstraintAngleViaPoint::ConstraintAngleViaPoint(Curve& acrv1, Curve& acrv2, Poi ConstraintAngleViaPoint::~ConstraintAngleViaPoint() { - delete crv1; crv1 = nullptr; - delete crv2; crv2 = nullptr; + delete crv1; + crv1 = nullptr; + delete crv2; + crv2 = nullptr; } void ConstraintAngleViaPoint::ReconstructGeomPointers() { - int cnt=0; - cnt++;//skip angle - we have an inline function for that - poa.x = pvec[cnt]; cnt++; - poa.y = pvec[cnt]; cnt++; - crv1->ReconstructOnNewPvec(pvec,cnt); - crv2->ReconstructOnNewPvec(pvec,cnt); - pvecChangedFlag=false; + int cnt = 0; + cnt++;// skip angle - we have an inline function for that + poa.x = pvec[cnt]; + cnt++; + poa.y = pvec[cnt]; + cnt++; + crv1->ReconstructOnNewPvec(pvec, cnt); + crv2->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; } ConstraintType ConstraintAngleViaPoint::getTypeId() @@ -2477,21 +2494,23 @@ double ConstraintAngleViaPoint::error() return scale * err; } -double ConstraintAngleViaPoint::grad(double *param) +double ConstraintAngleViaPoint::grad(double* param) { // first of all, check that we need to compute anything. if (findParamInPvec(param) == -1) return 0.0; - double deriv=0.; + double deriv = 0.; - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); - if (param == angle()) deriv += -1.0; + if (param == angle()) + deriv += -1.0; DeriVector2 n1 = crv1->CalculateNormal(poa, param); DeriVector2 n2 = crv2->CalculateNormal(poa, param); - deriv -= ( (-n1.dx) * n1.y / pow(n1.length(), 2) + n1.dy * n1.x / pow(n1.length(), 2) ); - deriv += ( (-n2.dx) * n2.y / pow(n2.length(), 2) + n2.dy * n2.x / pow(n2.length(), 2) ); + deriv -= ((-n1.dx) * n1.y / pow(n1.length(), 2) + n1.dy * n1.x / pow(n1.length(), 2)); + deriv += ((-n2.dx) * n2.y / pow(n2.length(), 2) + n2.dy * n2.x / pow(n2.length(), 2)); // use numeric for testing @@ -2541,9 +2560,12 @@ ConstraintSnell::ConstraintSnell(Curve& ray1, Curve& ray2, Curve& boundary, Poin ConstraintSnell::~ConstraintSnell() { - delete ray1; ray1 = nullptr; - delete ray2; ray2 = nullptr; - delete boundary; boundary = nullptr; + delete ray1; + ray1 = nullptr; + delete ray2; + ray2 = nullptr; + delete boundary; + boundary = nullptr; } void ConstraintSnell::ReconstructGeomPointers() @@ -2606,7 +2628,7 @@ double ConstraintSnell::error() return scale * err; } -double ConstraintSnell::grad(double *param) +double ConstraintSnell::grad(double* param) { // first of all, check that we need to compute anything. if (findParamInPvec(param) == -1) @@ -2652,7 +2674,7 @@ ConstraintEqualLineLength::ConstraintEqualLineLength(Line& l1, Line& l2) void ConstraintEqualLineLength::ReconstructGeomPointers() { - int i=0; + int i = 0; l1.ReconstructOnNewPvec(pvec, i); l2.ReconstructOnNewPvec(pvec, i); pvecChangedFlag = false; @@ -2668,14 +2690,15 @@ void ConstraintEqualLineLength::rescale(double coef) scale = coef * 1; } -void ConstraintEqualLineLength::errorgrad(double *err, double *grad, double *param) +void ConstraintEqualLineLength::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); - DeriVector2 p1 (l1.p1, param); - DeriVector2 p2 (l1.p2, param); - DeriVector2 p3 (l2.p1, param); - DeriVector2 p4 (l2.p2, param); + 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); @@ -2691,12 +2714,13 @@ void ConstraintEqualLineLength::errorgrad(double *err, double *grad, double *par 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. + // 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 apart when a parameter is fully constrained or just locked into a maximum/minimum + // 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 apart 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) @@ -2775,20 +2799,21 @@ void ConstraintC2CDistance::rescale(double coef) scale = coef * 1; } -void ConstraintC2CDistance::errorgrad(double *err, double *grad, double *param) +void ConstraintC2CDistance::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); - DeriVector2 ct1 (c1.center, param); - DeriVector2 ct2 (c2.center, param); + DeriVector2 ct1(c1.center, param); + DeriVector2 ct2(c2.center, param); DeriVector2 vector_ct12 = ct1.subtr(ct2); double length_ct12, dlength_ct12; length_ct12 = vector_ct12.length(dlength_ct12); - // outer case (defined as the centers of the circles are outside the center of the other circles) - // it may well be that the circles intersect. + // outer case (defined as the centers of the circles are outside the center of the other + // circles) it may well be that the circles intersect. if (length_ct12 >= *c1.rad && length_ct12 >= *c2.rad) { if (err) { *err = length_ct12 - (*c2.rad + *c1.rad + *distance()); @@ -2799,8 +2824,8 @@ void ConstraintC2CDistance::errorgrad(double *err, double *grad, double *param) } } else { - double * bigradius = (*c1.rad >= *c2.rad)?c1.rad:c2.rad; - double * smallradius = (*c1.rad >= *c2.rad)?c2.rad:c1.rad; + double* bigradius = (*c1.rad >= *c2.rad) ? c1.rad : c2.rad; + double* smallradius = (*c1.rad >= *c2.rad) ? c2.rad : c1.rad; double smallspan = *smallradius + length_ct12 + *distance(); @@ -2832,11 +2857,11 @@ void ConstraintC2CDistance::errorgrad(double *err, double *grad, double *param) double ConstraintC2CDistance::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; } -double ConstraintC2CDistance::grad(double *param) +double ConstraintC2CDistance::grad(double* param) { if (findParamInPvec(param) == -1) return 0.0; @@ -2849,7 +2874,7 @@ double ConstraintC2CDistance::grad(double *param) // -------------------------------------------------------- // ConstraintC2LDistance -ConstraintC2LDistance::ConstraintC2LDistance(Circle &c, Line &l, double *d) +ConstraintC2LDistance::ConstraintC2LDistance(Circle& c, Line& l, double* d) { this->d = d; pvec.push_back(d); @@ -2884,19 +2909,20 @@ void ConstraintC2LDistance::ReconstructGeomPointers() pvecChangedFlag = false; } -void ConstraintC2LDistance::errorgrad(double *err, double *grad, double *param) +void ConstraintC2LDistance::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); - DeriVector2 ct (circle.center, param); - DeriVector2 p1 (line.p1, param); - DeriVector2 p2 (line.p2, param); + 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) + // 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 area = v_line.crossProdNorm(v_p1ct, darea);// parallelogram oriented area double dlength; double length = v_line.length(dlength); @@ -2915,15 +2941,16 @@ 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; + double dh = (darea - h * dlength) / length; if (err) { *err = *distance() + *circle.rad - h; } else if (grad) { - if ( param == distance() || param == circle.rad) { + if (param == distance() || param == circle.rad) { *grad = 1.0; - } else { + } + else { *grad = -dh; } } @@ -2932,11 +2959,11 @@ void ConstraintC2LDistance::errorgrad(double *err, double *grad, double *param) double ConstraintC2LDistance::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; } -double ConstraintC2LDistance::grad(double *param) +double ConstraintC2LDistance::grad(double* param) { if (findParamInPvec(param) == -1) return 0.0; @@ -2947,4 +2974,4 @@ double ConstraintC2LDistance::grad(double *param) return deriv * scale; } -} //namespace GCS +}// namespace GCS diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index 3c5d4b3343..7173de74be 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -23,10 +23,12 @@ #ifndef PLANEGCS_CONSTRAINTS_H #define PLANEGCS_CONSTRAINTS_H -#include "Geo.h" #include "../../SketcherGlobal.h" +#include "Geo.h" -//#define _GCS_EXTRACT_SOLVER_SUBSYSTEM_ // This enables debugging code intended to extract information to file bug reports against Eigen, not for production code +// This enables debugging code intended to extract information to file bug reports against Eigen, +// not for production code +// #define _GCS_EXTRACT_SOLVER_SUBSYSTEM_ #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ #define _PROTECTED_UNLESS_EXTRACT_MODE_ public @@ -38,750 +40,1192 @@ namespace GCS { - /////////////////////////////////////// - // Constraints - /////////////////////////////////////// +/////////////////////////////////////// +// Constraints +/////////////////////////////////////// - enum ConstraintType { - None = 0, - Equal = 1, - Difference = 2, - P2PDistance = 3, - P2PAngle = 4, - P2LDistance = 5, - PointOnLine = 6, - PointOnPerpBisector = 7, - Parallel = 8, - Perpendicular = 9, - L2LAngle = 10, - MidpointOnLine = 11, - TangentCircumf = 12, - PointOnEllipse = 13, - TangentEllipseLine = 14, - InternalAlignmentPoint2Ellipse = 15, - EqualMajorAxesConic = 16, - EllipticalArcRangeToEndPoints = 17, - AngleViaPoint = 18, - Snell = 19, - CurveValue = 20, - PointOnHyperbola = 21, - InternalAlignmentPoint2Hyperbola = 22, - PointOnParabola = 23, - EqualFocalDistance = 24, - EqualLineLength = 25, - CenterOfGravity = 26, - WeightedLinearCombination = 27, - SlopeAtBSplineKnot = 28, - PointOnBSpline = 29, - C2CDistance = 30, - C2LDistance = 31 - }; +enum ConstraintType +{ + None = 0, + Equal = 1, + Difference = 2, + P2PDistance = 3, + P2PAngle = 4, + P2LDistance = 5, + PointOnLine = 6, + PointOnPerpBisector = 7, + Parallel = 8, + Perpendicular = 9, + L2LAngle = 10, + MidpointOnLine = 11, + TangentCircumf = 12, + PointOnEllipse = 13, + TangentEllipseLine = 14, + InternalAlignmentPoint2Ellipse = 15, + EqualMajorAxesConic = 16, + EllipticalArcRangeToEndPoints = 17, + AngleViaPoint = 18, + Snell = 19, + CurveValue = 20, + PointOnHyperbola = 21, + InternalAlignmentPoint2Hyperbola = 22, + PointOnParabola = 23, + EqualFocalDistance = 24, + EqualLineLength = 25, + CenterOfGravity = 26, + WeightedLinearCombination = 27, + SlopeAtBSplineKnot = 28, + PointOnBSpline = 29, + C2CDistance = 30, + C2LDistance = 31 +}; - enum InternalAlignmentType { - EllipsePositiveMajorX = 0, - EllipsePositiveMajorY = 1, - EllipseNegativeMajorX = 2, - EllipseNegativeMajorY = 3, - EllipsePositiveMinorX = 4, - EllipsePositiveMinorY = 5, - EllipseNegativeMinorX = 6, - EllipseNegativeMinorY = 7, - EllipseFocus2X = 8, - EllipseFocus2Y = 9, - HyperbolaPositiveMajorX = 10, - HyperbolaPositiveMajorY = 11, - HyperbolaNegativeMajorX = 12, - HyperbolaNegativeMajorY = 13, - HyperbolaPositiveMinorX = 14, - HyperbolaPositiveMinorY = 15, - HyperbolaNegativeMinorX = 16, - HyperbolaNegativeMinorY = 17 - }; +enum InternalAlignmentType +{ + EllipsePositiveMajorX = 0, + EllipsePositiveMajorY = 1, + EllipseNegativeMajorX = 2, + EllipseNegativeMajorY = 3, + EllipsePositiveMinorX = 4, + EllipsePositiveMinorY = 5, + EllipseNegativeMinorX = 6, + EllipseNegativeMinorY = 7, + EllipseFocus2X = 8, + EllipseFocus2Y = 9, + HyperbolaPositiveMajorX = 10, + HyperbolaPositiveMajorY = 11, + HyperbolaNegativeMajorX = 12, + HyperbolaNegativeMajorY = 13, + HyperbolaPositiveMinorX = 14, + HyperbolaPositiveMinorY = 15, + HyperbolaNegativeMinorX = 16, + HyperbolaNegativeMinorY = 17 +}; - class SketcherExport Constraint +class SketcherExport Constraint +{ + +public: + enum class Alignment { - - public: - enum class Alignment { - NoInternalAlignment, - InternalAlignment - }; - - _PROTECTED_UNLESS_EXTRACT_MODE_: - VEC_pD origpvec; // is used only as a reference for redirecting and reverting pvec - VEC_pD pvec; - double scale; - int tag; - bool pvecChangedFlag; //indicates that pvec has changed and saved pointers must be reconstructed (currently used only in AngleViaPoint) - bool driving; - Alignment internalAlignment; - - public: - Constraint(); - virtual ~Constraint(){} - - inline VEC_pD params() { return pvec; } - - void redirectParams(const MAP_pD_pD & redirectionmap); - void revertParams(); - void setTag(int tagId) { tag = tagId; } - int getTag() { return tag; } - - void setDriving(bool isdriving) { driving = isdriving; } - bool isDriving() const { return driving; } - - void setInternalAlignment(Alignment isinternalalignment) { internalAlignment = isinternalalignment; } - Alignment isInternalAlignment() const { return internalAlignment; } - - virtual ConstraintType getTypeId(); - virtual void rescale(double coef=1.); - virtual double error(); - 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. - // Returns -1 if not found. - int findParamInPvec(double* param); + NoInternalAlignment, + InternalAlignment }; - // Equal - class ConstraintEqual : public Constraint - { - private: - double ratio; - inline double* param1() { return pvec[0]; } - inline double* param2() { return pvec[1]; } - public: - ConstraintEqual(double *p1, double *p2, double p1p2ratio=1.0); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; + _PROTECTED_UNLESS_EXTRACT_MODE_ + : VEC_pD origpvec;// is used only as a reference for redirecting and reverting pvec + VEC_pD pvec; + double scale; + int tag; + // indicates that pvec has changed and saved pointers must be reconstructed (currently used only + // in AngleViaPoint) + bool pvecChangedFlag; + bool driving; + Alignment internalAlignment; - // Center of Gravity - class ConstraintCenterOfGravity : public Constraint - { - inline double* thecenter() { return pvec[0]; } - inline double* pointat(size_t i) { return pvec[1 + i]; } - public: - /// Constrains that the first parameter is center of gravity of rest - /// Let `pvec = [q, p_1, p_2,...]`, and - /// `givenweights = [f_1, f_2,...]`, then this constraint ensures - /// `q = sum(p_i*f_i)`. - ConstraintCenterOfGravity(const std::vector& givenpvec, const std::vector& givenweights); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - private: - std::vector weights; - double numpoints; - }; +public: + Constraint(); + virtual ~Constraint() + {} - // Weighted Linear Combination - class ConstraintWeightedLinearCombination : public Constraint + inline VEC_pD params() { - inline double* thepoint() { return pvec[0]; } - inline double* poleat(size_t i) { return pvec[1 + i]; } - inline double* weightat(size_t i) { return pvec[1 + numpoles + i]; } - public: - /// Constrains that the first element in pvec is a linear combination - /// of the next numpoints elements in homogeneous coordinates with - /// weights given in the last numpoints elements. - /// Let `pvec = [q, p_1, p_2,... w_1, w_2,...]`, and - /// `givenfactors = [f_1, f_2,...]`, then this constraint ensures - /// `q*sum(w_i*f_i) = sum(p_i*w_i*f_i)`. - /// - /// This constraint is currently used to ensure that a B-spline knot - /// point remains at the position determined by it's poles. - /// In that case, `q` is the x (or y) coordinate of the knot, `p_i` are - /// the x (or y) coordinates of the poles, and `w_i` are their weights. - /// Finally, `f_i` are obtained using `BSpline::getLinCombFactor()`. - ConstraintWeightedLinearCombination(size_t givennumpoints, const std::vector& givenpvec, const std::vector& givenfactors); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - private: - std::vector factors; - size_t numpoles; - }; + return pvec; + } - // Slope at knot - class ConstraintSlopeAtBSplineKnot : public Constraint + void redirectParams(const MAP_pD_pD& redirectionmap); + void revertParams(); + void setTag(int tagId) { - private: - inline double* polexat(size_t i) { return pvec[i]; } - inline double* poleyat(size_t i) { return pvec[numpoles + i]; } - inline double* weightat(size_t i) { return pvec[2*numpoles + i]; } - inline double* linep1x() { return pvec[3*numpoles + 0]; } - inline double* linep1y() { return pvec[3*numpoles + 1]; } - inline double* linep2x() { return pvec[3*numpoles + 2]; } - inline double* linep2y() { return pvec[3*numpoles + 3]; } - public: - // TODO: Should be able to make the geometries passed const - // Constrains the slope at a (C1 continuous) knot of the b-spline - ConstraintSlopeAtBSplineKnot(BSpline& b, Line& l, size_t knotindex); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - private: - std::vector factors; - std::vector slopefactors; - size_t numpoles; - }; + tag = tagId; + } + int getTag() + { + return tag; + } - // Point On BSpline - class ConstraintPointOnBSpline : public Constraint + void setDriving(bool isdriving) { - private: - inline double* thepoint() { return pvec[0]; } - // TODO: better name because param has a different meaning here? - inline double* theparam() { return pvec[1]; } - inline double* poleat(size_t i) { return pvec[2 + (startpole + i) % bsp.poles.size()]; } - inline double* weightat(size_t i) { return pvec[2 + bsp.poles.size() + (startpole + i) % bsp.weights.size()]; } - void setStartPole(double u); - public: - /// TODO: Explain how it's provided - /// coordidx = 0 if x, 1 if y - ConstraintPointOnBSpline(double* point, double* initparam, int coordidx, BSpline& b); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - size_t numpoints; - BSpline& bsp; - size_t startpole; - }; + driving = isdriving; + } + bool isDriving() const + { + return driving; + } - // Difference - class ConstraintDifference : public Constraint + void setInternalAlignment(Alignment isinternalalignment) { - private: - inline double* param1() { return pvec[0]; } - inline double* param2() { return pvec[1]; } - inline double* difference() { return pvec[2]; } - public: - ConstraintDifference(double *p1, double *p2, double *d); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; + internalAlignment = isinternalalignment; + } + Alignment isInternalAlignment() const + { + return internalAlignment; + } - // P2PDistance - class ConstraintP2PDistance : public Constraint - { - private: - inline double* p1x() { return pvec[0]; } - inline double* p1y() { return pvec[1]; } - inline double* p2x() { return pvec[2]; } - inline double* p2y() { return pvec[3]; } - inline double* distance() { return pvec[4]; } - public: - ConstraintP2PDistance(Point &p1, Point &p2, double *d); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintP2PDistance(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - double maxStep(MAP_pD_D &dir, double lim=1.) override; - }; + virtual ConstraintType getTypeId(); + virtual void rescale(double coef = 1.); + virtual double error(); + 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. + // Returns -1 if not found. + int findParamInPvec(double* param); +}; - // P2PAngle - class ConstraintP2PAngle : public Constraint +// Equal +class ConstraintEqual: public Constraint +{ +private: + double ratio; + inline double* param1() { - private: - inline double* p1x() { return pvec[0]; } - inline double* p1y() { return pvec[1]; } - inline double* p2x() { return pvec[2]; } - inline double* p2y() { return pvec[3]; } - inline double* angle() { return pvec[4]; } - double da; - public: - ConstraintP2PAngle(Point &p1, Point &p2, double *a, double da_=0.); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintP2PAngle(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - double maxStep(MAP_pD_D &dir, double lim=1.) override; - }; + return pvec[0]; + } + inline double* param2() + { + return pvec[1]; + } - // P2LDistance - class ConstraintP2LDistance : public Constraint - { - private: - inline double* p0x() { return pvec[0]; } - inline double* p0y() { return pvec[1]; } - inline double* p1x() { return pvec[2]; } - inline double* p1y() { return pvec[3]; } - inline double* p2x() { return pvec[4]; } - inline double* p2y() { return pvec[5]; } - inline double* distance() { return pvec[6]; } - public: - ConstraintP2LDistance(Point &p, Line &l, double *d); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintP2LDistance(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - double maxStep(MAP_pD_D &dir, double lim=1.) override; - double abs(double darea); - }; +public: + ConstraintEqual(double* p1, double* p2, double p1p2ratio = 1.0); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; - // PointOnLine - class ConstraintPointOnLine : public Constraint +// Center of Gravity +class ConstraintCenterOfGravity: public Constraint +{ + inline double* thecenter() { - private: - inline double* p0x() { return pvec[0]; } - inline double* p0y() { return pvec[1]; } - inline double* p1x() { return pvec[2]; } - inline double* p1y() { return pvec[3]; } - inline double* p2x() { return pvec[4]; } - inline double* p2y() { return pvec[5]; } - public: - ConstraintPointOnLine(Point &p, Line &l); - ConstraintPointOnLine(Point &p, Point &lp1, Point &lp2); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintPointOnLine(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; + return pvec[0]; + } + inline double* pointat(size_t i) + { + return pvec[1 + i]; + } - // PointOnPerpBisector - class ConstraintPointOnPerpBisector : public Constraint - { - private: - inline double* p0x() { return pvec[0]; } - inline double* p0y() { return pvec[1]; } - inline double* p1x() { return pvec[2]; } - inline double* p1y() { return pvec[3]; } - inline double* p2x() { return pvec[4]; } - inline double* p2y() { return pvec[5]; } - void errorgrad(double *err, double *grad, double *param); - public: - ConstraintPointOnPerpBisector(Point &p, Line &l); - ConstraintPointOnPerpBisector(Point &p, Point &lp1, Point &lp2); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintPointOnPerpBisector(){}; - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; +public: + /// Constrains that the first parameter is center of gravity of rest + /// Let `pvec = [q, p_1, p_2,...]`, and + /// `givenweights = [f_1, f_2,...]`, then this constraint ensures + /// `q = sum(p_i*f_i)`. + ConstraintCenterOfGravity(const std::vector& givenpvec, + const std::vector& givenweights); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; - double error() override; - double grad(double *) override; - }; +private: + std::vector weights; + double numpoints; +}; - // Parallel - class ConstraintParallel : public Constraint +// Weighted Linear Combination +class ConstraintWeightedLinearCombination: public Constraint +{ + inline double* thepoint() { - private: - inline double* l1p1x() { return pvec[0]; } - inline double* l1p1y() { return pvec[1]; } - inline double* l1p2x() { return pvec[2]; } - inline double* l1p2y() { return pvec[3]; } - inline double* l2p1x() { return pvec[4]; } - inline double* l2p1y() { return pvec[5]; } - inline double* l2p2x() { return pvec[6]; } - inline double* l2p2y() { return pvec[7]; } - public: - ConstraintParallel(Line &l1, Line &l2); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintParallel(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; + return pvec[0]; + } + inline double* poleat(size_t i) + { + return pvec[1 + i]; + } + inline double* weightat(size_t i) + { + return pvec[1 + numpoles + i]; + } - // Perpendicular - class ConstraintPerpendicular : public Constraint - { - private: - inline double* l1p1x() { return pvec[0]; } - inline double* l1p1y() { return pvec[1]; } - inline double* l1p2x() { return pvec[2]; } - inline double* l1p2y() { return pvec[3]; } - inline double* l2p1x() { return pvec[4]; } - inline double* l2p1y() { return pvec[5]; } - inline double* l2p2x() { return pvec[6]; } - inline double* l2p2y() { return pvec[7]; } - public: - ConstraintPerpendicular(Line &l1, Line &l2); - ConstraintPerpendicular(Point &l1p1, Point &l1p2, Point &l2p1, Point &l2p2); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintPerpendicular(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; +public: + /// Constrains that the first element in pvec is a linear combination + /// of the next numpoints elements in homogeneous coordinates with + /// weights given in the last numpoints elements. + /// Let `pvec = [q, p_1, p_2,... w_1, w_2,...]`, and + /// `givenfactors = [f_1, f_2,...]`, then this constraint ensures + /// `q*sum(w_i*f_i) = sum(p_i*w_i*f_i)`. + /// + /// This constraint is currently used to ensure that a B-spline knot + /// point remains at the position determined by it's poles. + /// In that case, `q` is the x (or y) coordinate of the knot, `p_i` are + /// the x (or y) coordinates of the poles, and `w_i` are their weights. + /// Finally, `f_i` are obtained using `BSpline::getLinCombFactor()`. + ConstraintWeightedLinearCombination(size_t givennumpoints, + const std::vector& givenpvec, + const std::vector& givenfactors); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; - // L2LAngle - class ConstraintL2LAngle : public Constraint - { - private: - inline double* l1p1x() { return pvec[0]; } - inline double* l1p1y() { return pvec[1]; } - inline double* l1p2x() { return pvec[2]; } - inline double* l1p2y() { return pvec[3]; } - inline double* l2p1x() { return pvec[4]; } - inline double* l2p1y() { return pvec[5]; } - inline double* l2p2x() { return pvec[6]; } - inline double* l2p2y() { return pvec[7]; } - inline double* angle() { return pvec[8]; } - public: - ConstraintL2LAngle(Line &l1, Line &l2, double *a); - ConstraintL2LAngle(Point &l1p1, Point &l1p2, - Point &l2p1, Point &l2p2, double *a); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintL2LAngle(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - double maxStep(MAP_pD_D &dir, double lim=1.) override; - }; +private: + std::vector factors; + size_t numpoles; +}; - // MidpointOnLine - class ConstraintMidpointOnLine : public Constraint +// Slope at knot +class ConstraintSlopeAtBSplineKnot: public Constraint +{ +private: + inline double* polexat(size_t i) { - private: - inline double* l1p1x() { return pvec[0]; } - inline double* l1p1y() { return pvec[1]; } - inline double* l1p2x() { return pvec[2]; } - inline double* l1p2y() { return pvec[3]; } - inline double* l2p1x() { return pvec[4]; } - inline double* l2p1y() { return pvec[5]; } - inline double* l2p2x() { return pvec[6]; } - inline double* l2p2y() { return pvec[7]; } - public: - ConstraintMidpointOnLine(Line &l1, Line &l2); - ConstraintMidpointOnLine(Point &l1p1, Point &l1p2, Point &l2p1, Point &l2p2); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintMidpointOnLine(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; + return pvec[i]; + } + inline double* poleyat(size_t i) + { + return pvec[numpoles + i]; + } + inline double* weightat(size_t i) + { + return pvec[2 * numpoles + i]; + } + inline double* linep1x() + { + return pvec[3 * numpoles + 0]; + } + inline double* linep1y() + { + return pvec[3 * numpoles + 1]; + } + inline double* linep2x() + { + return pvec[3 * numpoles + 2]; + } + inline double* linep2y() + { + return pvec[3 * numpoles + 3]; + } - // TangentCircumf - class ConstraintTangentCircumf : public Constraint - { - private: - inline double* c1x() { return pvec[0]; } - inline double* c1y() { return pvec[1]; } - inline double* c2x() { return pvec[2]; } - inline double* c2y() { return pvec[3]; } - inline double* r1() { return pvec[4]; } - inline double* r2() { return pvec[5]; } - bool internal; - public: - ConstraintTangentCircumf(Point &p1, Point &p2, - double *rd1, double *rd2, bool internal_=false); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintTangentCircumf(bool internal_){internal=internal_;} - #endif - inline bool getInternal() {return internal;}; - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; - // PointOnEllipse - class ConstraintPointOnEllipse : public Constraint - { - private: - inline double* p1x() { return pvec[0]; } - inline double* p1y() { return pvec[1]; } - inline double* cx() { return pvec[2]; } - inline double* cy() { return pvec[3]; } - inline double* f1x() { return pvec[4]; } - inline double* f1y() { return pvec[5]; } - inline double* rmin() { return pvec[6]; } - public: - ConstraintPointOnEllipse(Point &p, Ellipse &e); - ConstraintPointOnEllipse(Point &p, ArcOfEllipse &a); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintPointOnEllipse(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; +public: + // TODO: Should be able to make the geometries passed const + // Constrains the slope at a (C1 continuous) knot of the b-spline + ConstraintSlopeAtBSplineKnot(BSpline& b, Line& l, size_t knotindex); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; - class ConstraintEllipseTangentLine : public Constraint - { - private: - Line l; - Ellipse e; - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa - void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. - public: - ConstraintEllipseTangentLine(Line &l, Ellipse &e); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; +private: + std::vector factors; + std::vector slopefactors; + size_t numpoles; +}; - class ConstraintInternalAlignmentPoint2Ellipse : public Constraint +// Point On BSpline +class ConstraintPointOnBSpline: public Constraint +{ +private: + inline double* thepoint() { - public: - ConstraintInternalAlignmentPoint2Ellipse(Ellipse &e, Point &p1, InternalAlignmentType alignmentType); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - private: - void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa - Ellipse e; - Point p; - InternalAlignmentType AlignmentType; - }; + return pvec[0]; + } + // TODO: better name because param has a different meaning here? + inline double* theparam() + { + return pvec[1]; + } + inline double* poleat(size_t i) + { + return pvec[2 + (startpole + i) % bsp.poles.size()]; + } + inline double* weightat(size_t i) + { + return pvec[2 + bsp.poles.size() + (startpole + i) % bsp.weights.size()]; + } + void setStartPole(double u); - class ConstraintInternalAlignmentPoint2Hyperbola : public Constraint - { - public: - ConstraintInternalAlignmentPoint2Hyperbola(Hyperbola &e, Point &p1, InternalAlignmentType alignmentType); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - private: - void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa - Hyperbola e; - Point p; - InternalAlignmentType AlignmentType; - }; +public: + /// TODO: Explain how it's provided + /// coordidx = 0 if x, 1 if y + ConstraintPointOnBSpline(double* point, double* initparam, int coordidx, BSpline& b); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + size_t numpoints; + BSpline& bsp; + size_t startpole; +}; - class ConstraintEqualMajorAxesConic : public Constraint +// Difference +class ConstraintDifference: public Constraint +{ +private: + inline double* param1() { - private: - MajorRadiusConic * e1; - MajorRadiusConic * e2; - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa - void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. - public: - ConstraintEqualMajorAxesConic(MajorRadiusConic * a1, MajorRadiusConic * a2); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; + return pvec[0]; + } + inline double* param2() + { + return pvec[1]; + } + inline double* difference() + { + return pvec[2]; + } - class ConstraintEqualFocalDistance : public Constraint - { - private: - ArcOfParabola * e1; - ArcOfParabola * e2; - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa - void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. - public: - ConstraintEqualFocalDistance(ArcOfParabola * a1, ArcOfParabola * a2); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; +public: + ConstraintDifference(double* p1, double* p2, double* d); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; - class ConstraintCurveValue : public Constraint +// P2PDistance +class ConstraintP2PDistance: public Constraint +{ +private: + inline double* p1x() { - private: - inline double* pcoord() { return pvec[2]; } //defines, which coordinate of point is being constrained by this constraint - inline double* u() { return pvec[3]; } - void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa - Curve* crv; - Point p; - public: - /** - * @brief ConstraintCurveValue: solver constraint that ties parameter value with point coordinates, according to curve's parametric equation. - * @param p : endpoint to be constrained - * @param pcoord : pointer to point coordinate to be constrained. Must be either p.x or p.y - * @param crv : the curve (crv->Value() must be functional) - * @param u : pointer to u parameter corresponding to the point - */ - ConstraintCurveValue(Point &p, double* pcoord, Curve& crv, double* u); - ~ConstraintCurveValue() override; - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - double maxStep(MAP_pD_D &dir, double lim=1.) override; - }; + return pvec[0]; + } + inline double* p1y() + { + return pvec[1]; + } + inline double* p2x() + { + return pvec[2]; + } + inline double* p2y() + { + return pvec[3]; + } + inline double* distance() + { + return pvec[4]; + } - // PointOnHyperbola - class ConstraintPointOnHyperbola : public Constraint - { - private: - inline double* p1x() { return pvec[0]; } - inline double* p1y() { return pvec[1]; } - inline double* cx() { return pvec[2]; } - inline double* cy() { return pvec[3]; } - inline double* f1x() { return pvec[4]; } - inline double* f1y() { return pvec[5]; } - inline double* rmin() { return pvec[6]; } - public: - ConstraintPointOnHyperbola(Point &p, Hyperbola &e); - ConstraintPointOnHyperbola(Point &p, ArcOfHyperbola &a); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintPointOnHyperbola(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; +public: + ConstraintP2PDistance(Point& p1, Point& p2, double* d); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintP2PDistance() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; +}; - // PointOnParabola - class ConstraintPointOnParabola : public Constraint +// P2PAngle +class ConstraintP2PAngle: public Constraint +{ +private: + inline double* p1x() { - private: - void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa - Parabola* parab; - Point p; - public: - ConstraintPointOnParabola(Point &p, Parabola &e); - ConstraintPointOnParabola(Point &p, ArcOfParabola &a); + return pvec[0]; + } + inline double* p1y() + { + return pvec[1]; + } + inline double* p2x() + { + return pvec[2]; + } + inline double* p2y() + { + return pvec[3]; + } + inline double* angle() + { + return pvec[4]; + } + double da; + +public: + ConstraintP2PAngle(Point& p1, Point& p2, double* a, double da_ = 0.); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintP2PAngle() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; +}; + +// P2LDistance +class ConstraintP2LDistance: public Constraint +{ +private: + inline double* p0x() + { + return pvec[0]; + } + inline double* p0y() + { + return pvec[1]; + } + inline double* p1x() + { + return pvec[2]; + } + inline double* p1y() + { + return pvec[3]; + } + inline double* p2x() + { + return pvec[4]; + } + inline double* p2y() + { + return pvec[5]; + } + inline double* distance() + { + return pvec[6]; + } + +public: + ConstraintP2LDistance(Point& p, Line& l, double* d); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintP2LDistance() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; + double abs(double darea); +}; + +// PointOnLine +class ConstraintPointOnLine: public Constraint +{ +private: + inline double* p0x() + { + return pvec[0]; + } + inline double* p0y() + { + return pvec[1]; + } + inline double* p1x() + { + return pvec[2]; + } + inline double* p1y() + { + return pvec[3]; + } + inline double* p2x() + { + return pvec[4]; + } + inline double* p2y() + { + return pvec[5]; + } + +public: + ConstraintPointOnLine(Point& p, Line& l); + ConstraintPointOnLine(Point& p, Point& lp1, Point& lp2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnLine() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// PointOnPerpBisector +class ConstraintPointOnPerpBisector: public Constraint +{ +private: + inline double* p0x() + { + return pvec[0]; + } + inline double* p0y() + { + return pvec[1]; + } + inline double* p1x() + { + return pvec[2]; + } + inline double* p1y() + { + return pvec[3]; + } + inline double* p2x() + { + return pvec[4]; + } + inline double* p2y() + { + return pvec[5]; + } + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintPointOnPerpBisector(Point& p, Line& l); + ConstraintPointOnPerpBisector(Point& p, Point& lp1, Point& lp2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnPerpBisector() {}; +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + + double error() override; + double grad(double*) override; +}; + +// Parallel +class ConstraintParallel: public Constraint +{ +private: + inline double* l1p1x() + { + return pvec[0]; + } + inline double* l1p1y() + { + return pvec[1]; + } + inline double* l1p2x() + { + return pvec[2]; + } + inline double* l1p2y() + { + return pvec[3]; + } + inline double* l2p1x() + { + return pvec[4]; + } + inline double* l2p1y() + { + return pvec[5]; + } + inline double* l2p2x() + { + return pvec[6]; + } + inline double* l2p2y() + { + return pvec[7]; + } + +public: + ConstraintParallel(Line& l1, Line& l2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintParallel() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// Perpendicular +class ConstraintPerpendicular: public Constraint +{ +private: + inline double* l1p1x() + { + return pvec[0]; + } + inline double* l1p1y() + { + return pvec[1]; + } + inline double* l1p2x() + { + return pvec[2]; + } + inline double* l1p2y() + { + return pvec[3]; + } + inline double* l2p1x() + { + return pvec[4]; + } + inline double* l2p1y() + { + return pvec[5]; + } + inline double* l2p2x() + { + return pvec[6]; + } + inline double* l2p2y() + { + return pvec[7]; + } + +public: + ConstraintPerpendicular(Line& l1, Line& l2); + ConstraintPerpendicular(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPerpendicular() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// L2LAngle +class ConstraintL2LAngle: public Constraint +{ +private: + inline double* l1p1x() + { + return pvec[0]; + } + inline double* l1p1y() + { + return pvec[1]; + } + inline double* l1p2x() + { + return pvec[2]; + } + inline double* l1p2y() + { + return pvec[3]; + } + inline double* l2p1x() + { + return pvec[4]; + } + inline double* l2p1y() + { + return pvec[5]; + } + inline double* l2p2x() + { + return pvec[6]; + } + inline double* l2p2y() + { + return pvec[7]; + } + inline double* angle() + { + return pvec[8]; + } + +public: + ConstraintL2LAngle(Line& l1, Line& l2, double* a); + ConstraintL2LAngle(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, double* a); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintL2LAngle() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; +}; + +// MidpointOnLine +class ConstraintMidpointOnLine: public Constraint +{ +private: + inline double* l1p1x() + { + return pvec[0]; + } + inline double* l1p1y() + { + return pvec[1]; + } + inline double* l1p2x() + { + return pvec[2]; + } + inline double* l1p2y() + { + return pvec[3]; + } + inline double* l2p1x() + { + return pvec[4]; + } + inline double* l2p1y() + { + return pvec[5]; + } + inline double* l2p2x() + { + return pvec[6]; + } + inline double* l2p2y() + { + return pvec[7]; + } + +public: + ConstraintMidpointOnLine(Line& l1, Line& l2); + ConstraintMidpointOnLine(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintMidpointOnLine() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// TangentCircumf +class ConstraintTangentCircumf: public Constraint +{ +private: + inline double* c1x() + { + return pvec[0]; + } + inline double* c1y() + { + return pvec[1]; + } + inline double* c2x() + { + return pvec[2]; + } + inline double* c2y() + { + return pvec[3]; + } + inline double* r1() + { + return pvec[4]; + } + inline double* r2() + { + return pvec[5]; + } + bool internal; + +public: + ConstraintTangentCircumf(Point& p1, Point& p2, double* rd1, double* rd2, + bool internal_ = false); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintTangentCircumf(bool internal_) + { + internal = internal_; + } +#endif + inline bool getInternal() + { + return internal; + }; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; +// PointOnEllipse +class ConstraintPointOnEllipse: public Constraint +{ +private: + inline double* p1x() + { + return pvec[0]; + } + inline double* p1y() + { + return pvec[1]; + } + inline double* cx() + { + return pvec[2]; + } + inline double* cy() + { + return pvec[3]; + } + inline double* f1x() + { + return pvec[4]; + } + inline double* f1y() + { + return pvec[5]; + } + inline double* rmin() + { + return pvec[6]; + } + +public: + ConstraintPointOnEllipse(Point& p, Ellipse& e); + ConstraintPointOnEllipse(Point& p, ArcOfEllipse& a); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnEllipse() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintEllipseTangentLine: public Constraint +{ +private: + Line l; + Ellipse e; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintEllipseTangentLine(Line& l, Ellipse& e); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintInternalAlignmentPoint2Ellipse: public Constraint +{ +public: + ConstraintInternalAlignmentPoint2Ellipse(Ellipse& e, Point& p1, + InternalAlignmentType alignmentType); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + +private: + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + Ellipse e; + Point p; + InternalAlignmentType AlignmentType; +}; + +class ConstraintInternalAlignmentPoint2Hyperbola: public Constraint +{ +public: + ConstraintInternalAlignmentPoint2Hyperbola(Hyperbola& e, Point& p1, + InternalAlignmentType alignmentType); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + +private: + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + Hyperbola e; + Point p; + InternalAlignmentType AlignmentType; +}; + +class ConstraintEqualMajorAxesConic: public Constraint +{ +private: + MajorRadiusConic* e1; + MajorRadiusConic* e2; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintEqualMajorAxesConic(MajorRadiusConic* a1, MajorRadiusConic* a2); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintEqualFocalDistance: public Constraint +{ +private: + ArcOfParabola* e1; + ArcOfParabola* e2; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintEqualFocalDistance(ArcOfParabola* a1, ArcOfParabola* a2); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintCurveValue: public Constraint +{ +private: + // defines, which coordinate of point is being constrained by this constraint + inline double* pcoord() + { + return pvec[2]; + } + inline double* u() + { + return pvec[3]; + } + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + Curve* crv; + Point p; + +public: + /** + * @brief ConstraintCurveValue: solver constraint that ties parameter value with point + * coordinates, according to curve's parametric equation. + * @param p : endpoint to be constrained + * @param pcoord : pointer to point coordinate to be constrained. Must be either p.x or p.y + * @param crv : the curve (crv->Value() must be functional) + * @param u : pointer to u parameter corresponding to the point + */ + ConstraintCurveValue(Point& p, double* pcoord, Curve& crv, double* u); + ~ConstraintCurveValue() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; +}; + +// PointOnHyperbola +class ConstraintPointOnHyperbola: public Constraint +{ +private: + inline double* p1x() + { + return pvec[0]; + } + inline double* p1y() + { + return pvec[1]; + } + inline double* cx() + { + return pvec[2]; + } + inline double* cy() + { + return pvec[3]; + } + inline double* f1x() + { + return pvec[4]; + } + inline double* f1y() + { + return pvec[5]; + } + inline double* rmin() + { + return pvec[6]; + } + +public: + ConstraintPointOnHyperbola(Point& p, Hyperbola& e); + ConstraintPointOnHyperbola(Point& p, ArcOfHyperbola& a); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnHyperbola() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// PointOnParabola +class ConstraintPointOnParabola: public Constraint +{ +private: + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + Parabola* parab; + Point p; + +public: + ConstraintPointOnParabola(Point& p, Parabola& e); + ConstraintPointOnParabola(Point& p, ArcOfParabola& a); ~ConstraintPointOnParabola() override; - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - inline ConstraintPointOnParabola(){} - #endif - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnParabola() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; - class ConstraintAngleViaPoint : public Constraint +class ConstraintAngleViaPoint: public Constraint +{ +private: + inline double* angle() { - private: - inline double* angle() { return pvec[0]; }; - Curve* crv1; - Curve* crv2; - //These two pointers hold copies of the curves that were passed on - // constraint creation. The curves must be deleted upon destruction of - // the constraint. It is necessary to have copies, since messing with - // original objects that were passed is a very bad idea (but messing is - // necessary, because we need to support redirectParams()/revertParams - // functions. - //The pointers in the curves need to be reconstructed if pvec was redirected - // (test pvecChangedFlag variable before use!) - Point poa;//poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. The point is easily shallow-copied by C++, so no pointer type here and no delete is necessary. - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa - public: - ConstraintAngleViaPoint(Curve &acrv1, Curve &acrv2, Point p, double* angle); - ~ConstraintAngleViaPoint() override; - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; + return pvec[0]; }; + Curve* crv1; + Curve* crv2; + // These two pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + // poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. The point is + // easily shallow-copied by C++, so no pointer type here and no delete is necessary. + Point poa; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); - class ConstraintSnell : public Constraint //snell's law angles constrainer. Point needs to lie on all three curves to be constraied. +public: + ConstraintAngleViaPoint(Curve& acrv1, Curve& acrv2, Point p, double* angle); + ~ConstraintAngleViaPoint() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// snell's law angles constrainer. Point needs to lie on all three curves to be constraied. +class ConstraintSnell: public Constraint +{ +private: + inline double* n1() { - private: - inline double* n1() { return pvec[0]; }; - inline double* n2() { return pvec[1]; }; - Curve* ray1; - Curve* ray2; - Curve* boundary; - //These pointers hold copies of the curves that were passed on - // constraint creation. The curves must be deleted upon destruction of - // the constraint. It is necessary to have copies, since messing with - // original objects that were passed is a very bad idea (but messing is - // necessary, because we need to support redirectParams()/revertParams - // functions. - //The pointers in the curves need to be reconstructed if pvec was redirected - // (test pvecChangedFlag variable before use!) - Point poa;//poa=point of refraction //needs to be reconstructed if pvec was redirected/reverted. The point is easily shallow-copied by C++, so no pointer type here and no delete is necessary. - bool flipn1, flipn2; - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa - void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. - public: - //n1dn2 = n1 divided by n2. from n1 to n2. flipn1 = true instructs to flip ray1's tangent - ConstraintSnell(Curve &ray1, Curve &ray2, Curve &boundary, Point p, double* n1, double* n2, bool flipn1, bool flipn2); - ~ConstraintSnell() override; - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; + return pvec[0]; }; - - class ConstraintEqualLineLength : public Constraint + inline double* n2() { - 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); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; + return pvec[1]; }; + Curve* ray1; + Curve* ray2; + Curve* boundary; + // These pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + // poa=point of refraction //needs to be reconstructed if pvec was redirected/reverted. The + // point is easily shallow-copied by C++, so no pointer type here and no delete is necessary. + Point poa; + bool flipn1, flipn2; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); - class ConstraintC2CDistance : public Constraint +public: + // n1dn2 = n1 divided by n2. from n1 to n2. flipn1 = true instructs to flip ray1's tangent + ConstraintSnell(Curve& ray1, Curve& ray2, Curve& boundary, Point p, double* n1, double* n2, + bool flipn1, bool flipn2); + ~ConstraintSnell() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintEqualLineLength: public Constraint +{ +private: + Line l1; + Line l2; + // writes pointers in pvec to the parameters of line1, line2 + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintEqualLineLength(Line& l1, Line& l2); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintC2CDistance: public Constraint +{ +private: + Circle c1; + Circle c2; + double* d; + inline double* distance() { - private: - Circle c1; - Circle c2; - double *d; - inline double* distance() { return pvec[0]; } - void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of c1, c2 - void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. - public: - ConstraintC2CDistance(Circle &c1, Circle &c2, double *d); - ConstraintType getTypeId() override; - void rescale(double coef=1.) override; - double error() override; - double grad(double *) override; - }; + return pvec[0]; + } + // writes pointers in pvec to the parameters of c1, c2 + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); - // C2LDistance - class ConstraintC2LDistance : public Constraint +public: + ConstraintC2CDistance(Circle& c1, Circle& c2, double* d); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// C2LDistance +class ConstraintC2LDistance: public Constraint +{ +private: + Circle circle; + Line line; + double* d; + inline double* distance() { - 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; - }; + return pvec[0]; + } + // writes pointers in pvec to the parameters of c, l + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); -} //namespace GCS +public: + ConstraintC2LDistance(Circle& c, Line& l, double* d); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; -#endif // PLANEGCS_CONSTRAINTS_H +}// 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 b3e73344e0..b8a98734e9 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -25,10 +25,10 @@ #pragma warning(disable : 4996) #endif -//#define _GCS_DEBUG -//#define _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX -//#define _DEBUG_TO_FILE // Many matrices surpass the report view string size. -//#define PROFILE_DIAGNOSE +// #define _GCS_DEBUG +// #define _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX +// #define _DEBUG_TO_FILE // Many matrices surpass the report view string size. +// #define PROFILE_DIAGNOSE #undef _GCS_DEBUG #undef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX #undef _DEBUG_TO_FILE @@ -36,7 +36,7 @@ // This has to be included BEFORE any EIGEN include // This format is Sage compatible, so you can just copy/paste the matrix into Sage #ifdef _GCS_DEBUG -#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(3,0,",",",\n","[","]","[","]") +#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(3, 0, ",", ",\n", "[", "]", "[", "]") /* Parameters: * * StreamPrecision, @@ -49,11 +49,11 @@ * const std::string & _matSuffix = "" )*/ #endif -#include #include #include -#include #include +#include +#include #include "GCS.h" #include "qp_eq.h" @@ -65,7 +65,6 @@ // eigen3.3 - // Extraction of Q matrix for Debugging used to crash #ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX #if EIGEN_VERSION >= 30304 @@ -73,20 +72,19 @@ #endif #endif -#if EIGEN_VERSION > 30290 // This regulates that only starting in Eigen 3.3, the problem with - // http://forum.freecad.org/viewtopic.php?f=3&t=4651&start=40 - // was solved in Eigen: - // http://forum.freecad.org/viewtopic.php?f=10&t=12769&start=60#p106492 - // https://forum.kde.org/viewtopic.php?f=74&t=129439 +#if EIGEN_VERSION > 30290// This regulates that only starting in Eigen 3.3, the problem with + // http://forum.freecad.org/viewtopic.php?f=3&t=4651&start=40 + // was solved in Eigen: + // http://forum.freecad.org/viewtopic.php?f=10&t=12769&start=60#p106492 + // https://forum.kde.org/viewtopic.php?f=74&t=129439 #define EIGEN_STOCK_FULLPIVLU_COMPUTE #endif -//#undef EIGEN_SPARSEQR_COMPATIBLE - +// #undef EIGEN_SPARSEQR_COMPATIBLE #ifdef EIGEN_SPARSEQR_COMPATIBLE - #include +#include #endif // _GCS_EXTRACT_SOLVER_SUBSYSTEM_ to be enabled in Constraints.h when needed. @@ -100,117 +98,119 @@ } #endif -#include #include +#include -#include #include +#include using MatrixIndexType = Eigen::FullPivHouseholderQR::IntDiagSizeVectorType; #ifndef EIGEN_STOCK_FULLPIVLU_COMPUTE -namespace Eigen { +namespace Eigen +{ - using MatrixdType = Matrix; +using MatrixdType = Matrix; - template<> - FullPivLU& FullPivLU::compute(const MatrixdType& matrix) - { - m_isInitialized = true; - m_lu = matrix; +template<> +FullPivLU& FullPivLU::compute(const MatrixdType& matrix) +{ + m_isInitialized = true; + m_lu = matrix; - const Index size = matrix.diagonalSize(); - const Index rows = matrix.rows(); - const Index cols = matrix.cols(); + const Index size = matrix.diagonalSize(); + const Index rows = matrix.rows(); + const Index cols = matrix.cols(); - // will store the transpositions, before we accumulate them at the end. - // can't accumulate on-the-fly because that will be done in reverse order for the rows. - m_rowsTranspositions.resize(matrix.rows()); - m_colsTranspositions.resize(matrix.cols()); - Index number_of_transpositions = - 0;// number of NONTRIVIAL transpositions, i.e. m_rowsTranspositions[i]!=i + // will store the transpositions, before we accumulate them at the end. + // can't accumulate on-the-fly because that will be done in reverse order for the rows. + m_rowsTranspositions.resize(matrix.rows()); + m_colsTranspositions.resize(matrix.cols()); + // number of NONTRIVIAL transpositions, i.e. m_rowsTranspositions[i]!=i + Index number_of_transpositions = 0; - m_nonzero_pivots = - size;// the generic case is that in which all pivots are nonzero (invertible case) - m_maxpivot = RealScalar(0); - RealScalar cutoff(0); + // the generic case is that in which all pivots are nonzero (invertible case) + m_nonzero_pivots = size; + m_maxpivot = RealScalar(0); + RealScalar cutoff(0); - for (Index k = 0; k < size; ++k) { - // First, we need to find the pivot. + for (Index k = 0; k < size; ++k) { + // First, we need to find the pivot. - // biggest coefficient in the remaining bottom-right corner (starting at row k, col k) - Index row_of_biggest_in_corner, col_of_biggest_in_corner; - RealScalar biggest_in_corner; - biggest_in_corner = m_lu.bottomRightCorner(rows - k, cols - k) - .cwiseAbs() - .maxCoeff(&row_of_biggest_in_corner, &col_of_biggest_in_corner); - row_of_biggest_in_corner += - k;// correct the values! since they were computed in the corner, - col_of_biggest_in_corner += k;// need to add k to them. + // biggest coefficient in the remaining bottom-right corner (starting at row k, col k) + Index row_of_biggest_in_corner, col_of_biggest_in_corner; + RealScalar biggest_in_corner; + biggest_in_corner = m_lu.bottomRightCorner(rows - k, cols - k) + .cwiseAbs() + .maxCoeff(&row_of_biggest_in_corner, &col_of_biggest_in_corner); - // when k==0, biggest_in_corner is the biggest coeff absolute value in the original - // matrix - if (k == 0) - cutoff = biggest_in_corner * NumTraits::epsilon(); + // correct the values! since they were computed in the corner, + row_of_biggest_in_corner += k; + col_of_biggest_in_corner += k;// need to add k to them. - // if the pivot (hence the corner) is "zero", terminate to avoid generating nan/inf - // values. Notice that using an exact comparison (biggest_in_corner==0) here, as - // Golub-van Loan do in their pseudo-code, results in numerical instability! The cutoff - // here has been validated by running the unit test 'lu' with many repetitions. - if (biggest_in_corner < cutoff) { - // before exiting, make sure to initialize the still uninitialized transpositions - // in a sane state without destroying what we already have. - m_nonzero_pivots = k; - for (Index i = k; i < size; ++i) { - m_rowsTranspositions.coeffRef(i) = i; - m_colsTranspositions.coeffRef(i) = i; - } - break; + // when k==0, biggest_in_corner is the biggest coeff absolute value in the original + // matrix + if (k == 0) + cutoff = biggest_in_corner * NumTraits::epsilon(); + + // if the pivot (hence the corner) is "zero", terminate to avoid generating nan/inf + // values. Notice that using an exact comparison (biggest_in_corner==0) here, as + // Golub-van Loan do in their pseudo-code, results in numerical instability! The cutoff + // here has been validated by running the unit test 'lu' with many repetitions. + if (biggest_in_corner < cutoff) { + // before exiting, make sure to initialize the still uninitialized transpositions + // in a sane state without destroying what we already have. + m_nonzero_pivots = k; + for (Index i = k; i < size; ++i) { + m_rowsTranspositions.coeffRef(i) = i; + m_colsTranspositions.coeffRef(i) = i; } - - if (biggest_in_corner > m_maxpivot) - m_maxpivot = biggest_in_corner; - - // Now that we've found the pivot, we need to apply the row/col swaps to - // bring it to the location (k,k). - - m_rowsTranspositions.coeffRef(k) = row_of_biggest_in_corner; - m_colsTranspositions.coeffRef(k) = col_of_biggest_in_corner; - if (k != row_of_biggest_in_corner) { - m_lu.row(k).swap(m_lu.row(row_of_biggest_in_corner)); - ++number_of_transpositions; - } - if (k != col_of_biggest_in_corner) { - m_lu.col(k).swap(m_lu.col(col_of_biggest_in_corner)); - ++number_of_transpositions; - } - - // Now that the pivot is at the right location, we update the remaining - // bottom-right corner by Gaussian elimination. - - if (k < rows - 1) - m_lu.col(k).tail(rows - k - 1) /= m_lu.coeff(k, k); - if (k < size - 1) - m_lu.block(k + 1, k + 1, rows - k - 1, cols - k - 1).noalias() -= - m_lu.col(k).tail(rows - k - 1) * m_lu.row(k).tail(cols - k - 1); + break; } - // the main loop is over, we still have to accumulate the transpositions to find the - // permutations P and Q + if (biggest_in_corner > m_maxpivot) + m_maxpivot = biggest_in_corner; - m_p.setIdentity(rows); - for(Index k = size-1; k >= 0; --k) - m_p.applyTranspositionOnTheRight(k, m_rowsTranspositions.coeff(k)); + // Now that we've found the pivot, we need to apply the row/col swaps to + // bring it to the location (k,k). - m_q.setIdentity(cols); - for(Index k = 0; k < size; ++k) - m_q.applyTranspositionOnTheRight(k, m_colsTranspositions.coeff(k)); + m_rowsTranspositions.coeffRef(k) = row_of_biggest_in_corner; + m_colsTranspositions.coeffRef(k) = col_of_biggest_in_corner; + if (k != row_of_biggest_in_corner) { + m_lu.row(k).swap(m_lu.row(row_of_biggest_in_corner)); + ++number_of_transpositions; + } + if (k != col_of_biggest_in_corner) { + m_lu.col(k).swap(m_lu.col(col_of_biggest_in_corner)); + ++number_of_transpositions; + } - m_det_pq = (number_of_transpositions%2) ? -1 : 1; - return *this; + // Now that the pivot is at the right location, we update the remaining + // bottom-right corner by Gaussian elimination. + + if (k < rows - 1) + m_lu.col(k).tail(rows - k - 1) /= m_lu.coeff(k, k); + if (k < size - 1) + m_lu.block(k + 1, k + 1, rows - k - 1, cols - k - 1).noalias() -= + m_lu.col(k).tail(rows - k - 1) * m_lu.row(k).tail(cols - k - 1); } -} // Eigen + // the main loop is over, we still have to accumulate the transpositions to find the + // permutations P and Q + + m_p.setIdentity(rows); + for (Index k = size - 1; k >= 0; --k) + m_p.applyTranspositionOnTheRight(k, m_rowsTranspositions.coeff(k)); + + m_q.setIdentity(cols); + for (Index k = 0; k < size; ++k) + m_q.applyTranspositionOnTheRight(k, m_colsTranspositions.coeff(k)); + + m_det_pq = (number_of_transpositions % 2) ? -1 : 1; + return *this; +} + +}// namespace Eigen #endif namespace GCS @@ -222,7 +222,7 @@ public: SolverReportingManager(SolverReportingManager const&) = delete; SolverReportingManager(SolverReportingManager&&) = delete; SolverReportingManager& operator=(SolverReportingManager const&) = delete; - SolverReportingManager& operator=(SolverReportingManager &&) = delete; + SolverReportingManager& operator=(SolverReportingManager&&) = delete; static SolverReportingManager& Manager(); @@ -252,9 +252,9 @@ private: inline void flushStream(); private: - #ifdef _DEBUG_TO_FILE +#ifdef _DEBUG_TO_FILE std::ofstream stream; - #endif +#endif }; SolverReportingManager::SolverReportingManager() @@ -264,28 +264,28 @@ SolverReportingManager::SolverReportingManager() SolverReportingManager::~SolverReportingManager() { - #ifdef _DEBUG_TO_FILE +#ifdef _DEBUG_TO_FILE stream.flush(); stream.close(); - #endif +#endif } void SolverReportingManager::initStream() { - #ifdef _DEBUG_TO_FILE - if(!stream.is_open()) { +#ifdef _DEBUG_TO_FILE + if (!stream.is_open()) { stream.open("GCS_debug.txt", std::ofstream::out | std::ofstream::app); } - #endif +#endif } void SolverReportingManager::flushStream() { - // Akwardly in some systems flushing does not force the write to the file, requiring a close - #ifdef _DEBUG_TO_FILE +// Akwardly in some systems flushing does not force the write to the file, requiring a close +#ifdef _DEBUG_TO_FILE stream.flush(); stream.close(); - #endif +#endif } SolverReportingManager& SolverReportingManager::Manager() @@ -302,25 +302,25 @@ void SolverReportingManager::LogToConsole(const std::string& str) void SolverReportingManager::LogToFile(const std::string& str) { - #ifdef _DEBUG_TO_FILE +#ifdef _DEBUG_TO_FILE initStream(); stream << str << std::endl; flushStream(); - #else - (void)(str); // silence unused parameter +#else + (void)(str);// silence unused parameter LogToConsole("Debugging to file not enabled!"); - #endif +#endif } void SolverReportingManager::LogString(const std::string& str) { LogToConsole(str); - #ifdef _DEBUG_TO_FILE +#ifdef _DEBUG_TO_FILE LogToFile(str); - #endif +#endif } void SolverReportingManager::LogQRSystemInformation(const System& system, int paramsNum, @@ -369,7 +369,7 @@ void SolverReportingManager::LogGroupOfConstraints( tempstream << "["; for (auto c : group) - tempstream << c->getTag() << " "; + tempstream << c->getTag() << " "; tempstream << "]" << '\n'; } @@ -403,7 +403,7 @@ void SolverReportingManager::LogGroupOfParameters(const std::string& str, tempstream << "["; for (auto p : parametergroups[i]) - tempstream << std::hex << p << " "; + tempstream << std::hex << p << " "; tempstream << "]" << '\n'; } @@ -422,7 +422,6 @@ void SolverReportingManager::LogMatrix(const std::string str, Eigen::MatrixXd ma tempstream << "]" << '\n'; LogString(tempstream.str()); - } void SolverReportingManager::LogMatrix(const std::string str, MatrixIndexType matrix) @@ -439,7 +438,7 @@ void SolverReportingManager::LogMatrix(const std::string str, MatrixIndexType ma #endif -using Graph = boost::adjacency_list ; +using Graph = boost::adjacency_list; /////////////////////////////////////// // Solver @@ -447,42 +446,42 @@ using Graph = boost::adjacency_list clist_) break; } case Perpendicular: { - ConstraintPerpendicular *oldconstr = static_cast(*constr); - newconstr = new ConstraintPerpendicular(*oldconstr); - break; + ConstraintPerpendicular *oldconstr = static_cast(*constr); newconstr = new ConstraintPerpendicular(*oldconstr); break; } case L2LAngle: { ConstraintL2LAngle *oldconstr = static_cast(*constr); @@ -550,9 +548,8 @@ System::System(std::vector clist_) break; } case MidpointOnLine: { - ConstraintMidpointOnLine *oldconstr = static_cast(*constr); - newconstr = new ConstraintMidpointOnLine(*oldconstr); - break; + ConstraintMidpointOnLine *oldconstr = static_cast(*constr); newconstr = new ConstraintMidpointOnLine(*oldconstr); break; } case None: break; @@ -594,45 +591,46 @@ void System::clear() void System::invalidatedDiagnosis() { - hasDiagnosis=false; + hasDiagnosis = false; pDependentParameters.clear(); pDependentParametersGroups.clear(); } void System::clearByTag(int tagId) { - std::vector constrvec; - for (std::vector::const_iterator - constr=clist.begin(); constr != clist.end(); ++constr) { + std::vector constrvec; + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr) { if ((*constr)->getTag() == tagId) constrvec.push_back(*constr); } - for (std::vector::const_iterator - constr=constrvec.begin(); constr != constrvec.end(); ++constr) { + for (std::vector::const_iterator constr = constrvec.begin(); + constr != constrvec.end(); + ++constr) { removeConstraint(*constr); } } -int System::addConstraint(Constraint *constr) +int System::addConstraint(Constraint* constr) { isInit = false; - if (constr->getTag() >= 0) // negatively tagged constraints have no impact - hasDiagnosis = false; // on the diagnosis + if (constr->getTag() >= 0)// negatively tagged constraints have no impact + hasDiagnosis = false; // on the diagnosis clist.push_back(constr); VEC_pD constr_params = constr->params(); - for (VEC_pD::const_iterator param=constr_params.begin(); - param != constr_params.end(); ++param) { -// jacobi.set(constr, *param, 0.); + for (VEC_pD::const_iterator param = constr_params.begin(); param != constr_params.end(); + ++param) { + // jacobi.set(constr, *param, 0.); c2p[constr].push_back(*param); p2c[*param].push_back(constr); } - return clist.size()-1; + return clist.size() - 1; } -void System::removeConstraint(Constraint *constr) +void System::removeConstraint(Constraint* constr) { - std::vector::iterator it; + std::vector::iterator it; it = std::find(clist.begin(), clist.end(), constr); if (it == clist.end()) return; @@ -643,15 +641,15 @@ void System::removeConstraint(Constraint *constr) clearSubSystems(); VEC_pD constr_params = c2p[constr]; - for (VEC_pD::const_iterator param=constr_params.begin(); - param != constr_params.end(); ++param) { - std::vector &constraints = p2c[*param]; + for (VEC_pD::const_iterator param = constr_params.begin(); param != constr_params.end(); + ++param) { + std::vector& constraints = p2c[*param]; it = std::find(constraints.begin(), constraints.end(), constr); constraints.erase(it); } c2p.erase(constr); - delete(constr); + delete (constr); } // basic constraints @@ -675,10 +673,10 @@ int System::addConstraintProportional(double* param1, double* param2, double rat return addConstraint(constr); } -int System::addConstraintDifference(double *param1, double *param2, - double *difference, int tagId, bool driving) +int System::addConstraintDifference(double* param1, double* param2, double* difference, int tagId, + bool driving) { - Constraint *constr = new ConstraintDifference(param1, param2, difference); + Constraint* constr = new ConstraintDifference(param1, param2, difference); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); @@ -693,155 +691,158 @@ int System::addConstraintP2PDistance(Point& p1, Point& p2, double* distance, int return addConstraint(constr); } -int System::addConstraintP2PAngle(Point &p1, Point &p2, double *angle, - double incrAngle, int tagId, bool driving) +int System::addConstraintP2PAngle(Point& p1, Point& p2, double* angle, double incrAngle, int tagId, + bool driving) { - Constraint *constr = new ConstraintP2PAngle(p1, p2, angle, incrAngle); + Constraint* constr = new ConstraintP2PAngle(p1, p2, angle, incrAngle); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintP2PAngle(Point &p1, Point &p2, double *angle, int /*tagId*/, bool driving) +int System::addConstraintP2PAngle(Point& p1, Point& p2, double* angle, int /*tagId*/, bool driving) { return addConstraintP2PAngle(p1, p2, angle, 0., 0, driving); } -int System::addConstraintP2LDistance(Point &p, Line &l, double *distance, int tagId, bool driving) +int System::addConstraintP2LDistance(Point& p, Line& l, double* distance, int tagId, bool driving) { - Constraint *constr = new ConstraintP2LDistance(p, l, distance); + Constraint* constr = new ConstraintP2LDistance(p, l, distance); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPointOnLine(Point &p, Line &l, int tagId, bool driving) +int System::addConstraintPointOnLine(Point& p, Line& l, int tagId, bool driving) { - Constraint *constr = new ConstraintPointOnLine(p, l); + Constraint* constr = new ConstraintPointOnLine(p, l); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPointOnLine(Point &p, Point &lp1, Point &lp2, int tagId, bool driving) +int System::addConstraintPointOnLine(Point& p, Point& lp1, Point& lp2, int tagId, bool driving) { - Constraint *constr = new ConstraintPointOnLine(p, lp1, lp2); + Constraint* constr = new ConstraintPointOnLine(p, lp1, lp2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPointOnPerpBisector(Point &p, Line &l, int tagId, bool driving) +int System::addConstraintPointOnPerpBisector(Point& p, Line& l, int tagId, bool driving) { - Constraint *constr = new ConstraintPointOnPerpBisector(p, l); + Constraint* constr = new ConstraintPointOnPerpBisector(p, l); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPointOnPerpBisector(Point &p, Point &lp1, Point &lp2, int tagId, bool driving) +int System::addConstraintPointOnPerpBisector(Point& p, Point& lp1, Point& lp2, int tagId, + bool driving) { - Constraint *constr = new ConstraintPointOnPerpBisector(p, lp1, lp2); + Constraint* constr = new ConstraintPointOnPerpBisector(p, lp1, lp2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintParallel(Line &l1, Line &l2, int tagId, bool driving) +int System::addConstraintParallel(Line& l1, Line& l2, int tagId, bool driving) { - Constraint *constr = new ConstraintParallel(l1, l2); + Constraint* constr = new ConstraintParallel(l1, l2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPerpendicular(Line &l1, Line &l2, int tagId, bool driving) +int System::addConstraintPerpendicular(Line& l1, Line& l2, int tagId, bool driving) { - Constraint *constr = new ConstraintPerpendicular(l1, l2); + Constraint* constr = new ConstraintPerpendicular(l1, l2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPerpendicular(Point &l1p1, Point &l1p2, - Point &l2p1, Point &l2p2, int tagId, bool driving) +int System::addConstraintPerpendicular(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, + int tagId, bool driving) { - Constraint *constr = new ConstraintPerpendicular(l1p1, l1p2, l2p1, l2p2); + Constraint* constr = new ConstraintPerpendicular(l1p1, l1p2, l2p1, l2p2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintL2LAngle(Line &l1, Line &l2, double *angle, int tagId, bool driving) +int System::addConstraintL2LAngle(Line& l1, Line& l2, double* angle, int tagId, bool driving) { - Constraint *constr = new ConstraintL2LAngle(l1, l2, angle); + Constraint* constr = new ConstraintL2LAngle(l1, l2, angle); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintL2LAngle(Point &l1p1, Point &l1p2, - Point &l2p1, Point &l2p2, double *angle, int tagId, bool driving) +int System::addConstraintL2LAngle(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, double* angle, + int tagId, bool driving) { - Constraint *constr = new ConstraintL2LAngle(l1p1, l1p2, l2p1, l2p2, angle); + Constraint* constr = new ConstraintL2LAngle(l1p1, l1p2, l2p1, l2p2, angle); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintAngleViaPoint(Curve &crv1, Curve &crv2, Point &p, double *angle, int tagId, bool driving) +int System::addConstraintAngleViaPoint(Curve& crv1, Curve& crv2, Point& p, double* angle, int tagId, + bool driving) { - Constraint *constr = new ConstraintAngleViaPoint(crv1, crv2, p, angle); + Constraint* constr = new ConstraintAngleViaPoint(crv1, crv2, p, angle); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintMidpointOnLine(Line &l1, Line &l2, int tagId, bool driving) +int System::addConstraintMidpointOnLine(Line& l1, Line& l2, int tagId, bool driving) { - Constraint *constr = new ConstraintMidpointOnLine(l1, l2); + Constraint* constr = new ConstraintMidpointOnLine(l1, l2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintMidpointOnLine(Point &l1p1, Point &l1p2, - Point &l2p1, Point &l2p2, int tagId, bool driving) +int System::addConstraintMidpointOnLine(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, + int tagId, bool driving) { - Constraint *constr = new ConstraintMidpointOnLine(l1p1, l1p2, l2p1, l2p2); + Constraint* constr = new ConstraintMidpointOnLine(l1p1, l1p2, l2p1, l2p2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintTangentCircumf(Point &p1, Point &p2, double *rad1, double *rad2, +int System::addConstraintTangentCircumf(Point& p1, Point& p2, double* rad1, double* rad2, bool internal, int tagId, bool driving) { - Constraint *constr = new ConstraintTangentCircumf(p1, p2, rad1, rad2, internal); + Constraint* constr = new ConstraintTangentCircumf(p1, p2, rad1, rad2, internal); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintTangentAtBSplineKnot(BSpline &b, Line &l, unsigned int knotindex, int tagId, bool driving) +int System::addConstraintTangentAtBSplineKnot(BSpline& b, Line& l, unsigned int knotindex, + int tagId, bool driving) { - Constraint *constr = new ConstraintSlopeAtBSplineKnot(b, l, knotindex); + Constraint* constr = new ConstraintSlopeAtBSplineKnot(b, l, knotindex); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintC2CDistance(Circle &c1, Circle &c2, double *dist, int tagId, bool driving) +int System::addConstraintC2CDistance(Circle& c1, Circle& c2, double* dist, int tagId, bool driving) { - Constraint *constr = new ConstraintC2CDistance(c1, c2, dist); + Constraint* constr = new ConstraintC2CDistance(c1, c2, dist); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintC2LDistance(Circle &c, Line &l, double *dist, int tagId, bool driving) +int System::addConstraintC2LDistance(Circle& c, Line& l, double* dist, int tagId, bool driving) { - Constraint *constr = new ConstraintC2LDistance(c, l, dist); + Constraint* constr = new ConstraintC2LDistance(c, l, dist); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); @@ -849,80 +850,81 @@ int System::addConstraintC2LDistance(Circle &c, Line &l, double *dist, int tagId // derived constraints -int System::addConstraintP2PCoincident(Point &p1, Point &p2, int tagId, bool driving) +int System::addConstraintP2PCoincident(Point& p1, Point& p2, int tagId, bool driving) { addConstraintEqual(p1.x, p2.x, tagId, driving); return addConstraintEqual(p1.y, p2.y, tagId, driving); } -int System::addConstraintHorizontal(Line &l, int tagId, bool driving) +int System::addConstraintHorizontal(Line& l, int tagId, bool driving) { return addConstraintEqual(l.p1.y, l.p2.y, tagId, driving); } -int System::addConstraintHorizontal(Point &p1, Point &p2, int tagId, bool driving) +int System::addConstraintHorizontal(Point& p1, Point& p2, int tagId, bool driving) { return addConstraintEqual(p1.y, p2.y, tagId, driving); } -int System::addConstraintVertical(Line &l, int tagId, bool driving) +int System::addConstraintVertical(Line& l, int tagId, bool driving) { return addConstraintEqual(l.p1.x, l.p2.x, tagId, driving); } -int System::addConstraintVertical(Point &p1, Point &p2, int tagId, bool driving) +int System::addConstraintVertical(Point& p1, Point& p2, int tagId, bool driving) { return addConstraintEqual(p1.x, p2.x, tagId, driving); } -int System::addConstraintCoordinateX(Point &p, double *x, int tagId, bool driving) +int System::addConstraintCoordinateX(Point& p, double* x, int tagId, bool driving) { return addConstraintEqual(p.x, x, tagId, driving); } -int System::addConstraintCoordinateY(Point &p, double *y, int tagId, bool driving) +int System::addConstraintCoordinateY(Point& p, double* y, int tagId, bool driving) { return addConstraintEqual(p.y, y, tagId, driving); } -int System::addConstraintArcRules(Arc &a, int tagId, bool driving) +int System::addConstraintArcRules(Arc& a, int tagId, bool driving) { addConstraintCurveValue(a.start, a, a.startAngle, tagId, driving); return addConstraintCurveValue(a.end, a, a.endAngle, tagId, driving); } -int System::addConstraintPointOnCircle(Point &p, Circle &c, int tagId, bool driving) +int System::addConstraintPointOnCircle(Point& p, Circle& c, int tagId, bool driving) { return addConstraintP2PDistance(p, c.center, c.rad, tagId, driving); } -int System::addConstraintPointOnEllipse(Point &p, Ellipse &e, int tagId, bool driving) +int System::addConstraintPointOnEllipse(Point& p, Ellipse& e, int tagId, bool driving) { - Constraint *constr = new ConstraintPointOnEllipse(p, e); + Constraint* constr = new ConstraintPointOnEllipse(p, e); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPointOnHyperbolicArc(Point &p, ArcOfHyperbola &e, int tagId, bool driving) +int System::addConstraintPointOnHyperbolicArc(Point& p, ArcOfHyperbola& e, int tagId, bool driving) { - Constraint *constr = new ConstraintPointOnHyperbola(p, e); + Constraint* constr = new ConstraintPointOnHyperbola(p, e); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPointOnParabolicArc(Point &p, ArcOfParabola &e, int tagId, bool driving) +int System::addConstraintPointOnParabolicArc(Point& p, ArcOfParabola& e, int tagId, bool driving) { - Constraint *constr = new ConstraintPointOnParabola(p, e); + Constraint* constr = new ConstraintPointOnParabola(p, e); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPointOnBSpline(Point &p, BSpline &b, double* pointparam, int tagId, bool driving) +int System::addConstraintPointOnBSpline(Point& p, BSpline& b, double* pointparam, int tagId, + bool driving) { - Constraint *constr = new ConstraintPointOnBSpline(p.x, pointparam, 0, b); + Constraint* constr = new ConstraintPointOnBSpline(p.x, pointparam, 0, b); constr->setTag(tagId); constr->setDriving(driving); addConstraint(constr); @@ -933,43 +935,43 @@ int System::addConstraintPointOnBSpline(Point &p, BSpline &b, double* pointparam return addConstraint(constr); } -int System::addConstraintArcOfEllipseRules(ArcOfEllipse &a, int tagId, bool driving) +int System::addConstraintArcOfEllipseRules(ArcOfEllipse& a, int tagId, bool driving) { - addConstraintCurveValue(a.start,a,a.startAngle, tagId, driving); - return addConstraintCurveValue(a.end,a,a.endAngle, tagId, driving); + addConstraintCurveValue(a.start, a, a.startAngle, tagId, driving); + return addConstraintCurveValue(a.end, a, a.endAngle, tagId, driving); } -int System::addConstraintCurveValue(Point &p, Curve &a, double *u, int tagId, bool driving) +int System::addConstraintCurveValue(Point& p, Curve& a, double* u, int tagId, bool driving) { - Constraint *constr = new ConstraintCurveValue(p,p.x,a,u); + Constraint* constr = new ConstraintCurveValue(p, p.x, a, u); constr->setTag(tagId); constr->setDriving(driving); addConstraint(constr); - constr = new ConstraintCurveValue(p,p.y,a,u); + constr = new ConstraintCurveValue(p, p.y, a, u); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintArcOfHyperbolaRules(ArcOfHyperbola &a, int tagId, bool driving) +int System::addConstraintArcOfHyperbolaRules(ArcOfHyperbola& a, int tagId, bool driving) { - addConstraintCurveValue(a.start,a,a.startAngle, tagId, driving); - return addConstraintCurveValue(a.end,a,a.endAngle, tagId, driving); + addConstraintCurveValue(a.start, a, a.startAngle, tagId, driving); + return addConstraintCurveValue(a.end, a, a.endAngle, tagId, driving); } -int System::addConstraintArcOfParabolaRules(ArcOfParabola &a, int tagId, bool driving) +int System::addConstraintArcOfParabolaRules(ArcOfParabola& a, int tagId, bool driving) { - addConstraintCurveValue(a.start,a,a.startAngle, tagId, driving); - return addConstraintCurveValue(a.end,a,a.endAngle, tagId, driving); + addConstraintCurveValue(a.start, a, a.startAngle, tagId, driving); + return addConstraintCurveValue(a.end, a, a.endAngle, tagId, driving); } -int System::addConstraintPointOnArc(Point &p, Arc &a, int tagId, bool driving) +int System::addConstraintPointOnArc(Point& p, Arc& a, int tagId, bool driving) { return addConstraintP2PDistance(p, a.center, a.rad, tagId, driving); } -int System::addConstraintPerpendicularLine2Arc(Point &p1, Point &p2, Arc &a, - int tagId, bool driving) +int System::addConstraintPerpendicularLine2Arc(Point& p1, Point& p2, Arc& a, int tagId, + bool driving) { addConstraintP2PCoincident(p2, a.start, tagId, driving); double dx = *(p2.x) - *(p1.x); @@ -980,8 +982,8 @@ int System::addConstraintPerpendicularLine2Arc(Point &p1, Point &p2, Arc &a, return addConstraintP2PAngle(p1, p2, a.startAngle, M_PI, tagId, driving); } -int System::addConstraintPerpendicularArc2Line(Arc &a, Point &p1, Point &p2, - int tagId, bool driving) +int System::addConstraintPerpendicularArc2Line(Arc& a, Point& p1, Point& p2, int tagId, + bool driving) { addConstraintP2PCoincident(p1, a.end, tagId, driving); double dx = *(p2.x) - *(p1.x); @@ -992,11 +994,11 @@ int System::addConstraintPerpendicularArc2Line(Arc &a, Point &p1, Point &p2, return addConstraintP2PAngle(p1, p2, a.endAngle, M_PI, tagId, driving); } -int System::addConstraintPerpendicularCircle2Arc(Point ¢er, double *radius, - Arc &a, int tagId, bool driving) +int System::addConstraintPerpendicularCircle2Arc(Point& center, double* radius, Arc& a, int tagId, + bool driving) { addConstraintP2PDistance(a.start, center, radius, tagId, driving); - double incrAngle = *(a.startAngle) < *(a.endAngle) ? M_PI/2 : -M_PI/2; + double incrAngle = *(a.startAngle) < *(a.endAngle) ? M_PI / 2 : -M_PI / 2; double tangAngle = *a.startAngle + incrAngle; double dx = *(a.start.x) - *(center.x); double dy = *(a.start.y) - *(center.y); @@ -1006,11 +1008,11 @@ int System::addConstraintPerpendicularCircle2Arc(Point ¢er, double *radius, return addConstraintP2PAngle(center, a.start, a.startAngle, -incrAngle, tagId, driving); } -int System::addConstraintPerpendicularArc2Circle(Arc &a, Point ¢er, - double *radius, int tagId, bool driving) +int System::addConstraintPerpendicularArc2Circle(Arc& a, Point& center, double* radius, int tagId, + bool driving) { addConstraintP2PDistance(a.end, center, radius, tagId, driving); - double incrAngle = *(a.startAngle) < *(a.endAngle) ? -M_PI/2 : M_PI/2; + double incrAngle = *(a.startAngle) < *(a.endAngle) ? -M_PI / 2 : M_PI / 2; double tangAngle = *a.endAngle + incrAngle; double dx = *(a.end.x) - *(center.x); double dy = *(a.end.y) - *(center.y); @@ -1020,150 +1022,147 @@ int System::addConstraintPerpendicularArc2Circle(Arc &a, Point ¢er, return addConstraintP2PAngle(center, a.end, a.endAngle, -incrAngle, tagId, driving); } -int System::addConstraintPerpendicularArc2Arc(Arc &a1, bool reverse1, - Arc &a2, bool reverse2, int tagId, bool driving) +int System::addConstraintPerpendicularArc2Arc(Arc& a1, bool reverse1, Arc& a2, bool reverse2, + int tagId, bool driving) { - Point &p1 = reverse1 ? a1.start : a1.end; - Point &p2 = reverse2 ? a2.end : a2.start; + Point& p1 = reverse1 ? a1.start : a1.end; + Point& p2 = reverse2 ? a2.end : a2.start; addConstraintP2PCoincident(p1, p2, tagId, driving); return addConstraintPerpendicular(a1.center, p1, a2.center, p2, tagId, driving); } -int System::addConstraintTangent(Line &l, Circle &c, int tagId, bool driving) +int System::addConstraintTangent(Line& l, Circle& c, int tagId, bool driving) { return addConstraintP2LDistance(c.center, l, c.rad, tagId, driving); } -int System::addConstraintTangent(Line &l, Ellipse &e, int tagId, bool driving) +int System::addConstraintTangent(Line& l, Ellipse& e, int tagId, bool driving) { - Constraint *constr = new ConstraintEllipseTangentLine(l, e); + Constraint* constr = new ConstraintEllipseTangentLine(l, e); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintTangent(Line &l, Arc &a, int tagId, bool driving) +int System::addConstraintTangent(Line& l, Arc& a, int tagId, bool driving) { return addConstraintP2LDistance(a.center, l, a.rad, tagId, driving); } -int System::addConstraintTangent(Circle &c1, Circle &c2, int tagId, bool driving) +int System::addConstraintTangent(Circle& c1, Circle& c2, int tagId, bool driving) { double dx = *(c2.center.x) - *(c1.center.x); double dy = *(c2.center.y) - *(c1.center.y); - double d = sqrt(dx*dx + dy*dy); - return addConstraintTangentCircumf(c1.center, c2.center, c1.rad, c2.rad, - (d < *c1.rad || d < *c2.rad), tagId, driving); + double d = sqrt(dx * dx + dy * dy); + return addConstraintTangentCircumf( + c1.center, c2.center, c1.rad, c2.rad, (d < *c1.rad || d < *c2.rad), tagId, driving); } -int System::addConstraintTangent(Arc &a1, Arc &a2, int tagId, bool driving) +int System::addConstraintTangent(Arc& a1, Arc& a2, int tagId, bool driving) { double dx = *(a2.center.x) - *(a1.center.x); double dy = *(a2.center.y) - *(a1.center.y); - double d = sqrt(dx*dx + dy*dy); - return addConstraintTangentCircumf(a1.center, a2.center, a1.rad, a2.rad, - (d < *a1.rad || d < *a2.rad), tagId, driving); + double d = sqrt(dx * dx + dy * dy); + return addConstraintTangentCircumf( + a1.center, a2.center, a1.rad, a2.rad, (d < *a1.rad || d < *a2.rad), tagId, driving); } -int System::addConstraintTangent(Circle &c, Arc &a, int tagId, bool driving) +int System::addConstraintTangent(Circle& c, Arc& a, int tagId, bool driving) { double dx = *(a.center.x) - *(c.center.x); double dy = *(a.center.y) - *(c.center.y); - double d = sqrt(dx*dx + dy*dy); - return addConstraintTangentCircumf(c.center, a.center, c.rad, a.rad, - (d < *c.rad || d < *a.rad), tagId, driving); + double d = sqrt(dx * dx + dy * dy); + return addConstraintTangentCircumf( + c.center, a.center, c.rad, a.rad, (d < *c.rad || d < *a.rad), tagId, driving); } -int System::addConstraintCircleRadius(Circle &c, double *radius, int tagId, bool driving) +int System::addConstraintCircleRadius(Circle& c, double* radius, int tagId, bool driving) { return addConstraintEqual(c.rad, radius, tagId, driving); } -int System::addConstraintArcRadius(Arc &a, double *radius, int tagId, bool driving) +int System::addConstraintArcRadius(Arc& a, double* radius, int tagId, bool driving) { return addConstraintEqual(a.rad, radius, tagId, driving); } -int System::addConstraintCircleDiameter(Circle &c, double *diameter, int tagId, bool driving) +int System::addConstraintCircleDiameter(Circle& c, double* diameter, int tagId, bool driving) { return addConstraintProportional(c.rad, diameter, 0.5, tagId, driving); } -int System::addConstraintArcDiameter(Arc &a, double *diameter, int tagId, bool driving) +int System::addConstraintArcDiameter(Arc& a, double* diameter, int tagId, bool driving) { return addConstraintProportional(a.rad, diameter, 0.5, tagId, driving); } -int System::addConstraintEqualLength(Line &l1, Line &l2, int tagId, bool driving) +int System::addConstraintEqualLength(Line& l1, Line& l2, int tagId, bool driving) { - Constraint *constr = new ConstraintEqualLineLength(l1, l2); + Constraint* constr = new ConstraintEqualLineLength(l1, l2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintEqualRadius(Circle &c1, Circle &c2, int tagId, bool driving) +int System::addConstraintEqualRadius(Circle& c1, Circle& c2, int tagId, bool driving) { return addConstraintEqual(c1.rad, c2.rad, tagId, driving); } -int System::addConstraintEqualRadii(Ellipse &e1, Ellipse &e2, int tagId, bool driving) +int System::addConstraintEqualRadii(Ellipse& e1, Ellipse& e2, int tagId, bool driving) { addConstraintEqual(e1.radmin, e2.radmin, tagId, driving); - Constraint *constr = new ConstraintEqualMajorAxesConic(&e1,&e2); + Constraint* constr = new ConstraintEqualMajorAxesConic(&e1, &e2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintEqualRadii(ArcOfHyperbola &a1, ArcOfHyperbola &a2, int tagId, bool driving) +int System::addConstraintEqualRadii(ArcOfHyperbola& a1, ArcOfHyperbola& a2, int tagId, bool driving) { addConstraintEqual(a1.radmin, a2.radmin, tagId, driving); - Constraint *constr = new ConstraintEqualMajorAxesConic(&a1,&a2); + Constraint* constr = new ConstraintEqualMajorAxesConic(&a1, &a2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintEqualFocus(ArcOfParabola &a1, ArcOfParabola &a2, int tagId, bool driving) +int System::addConstraintEqualFocus(ArcOfParabola& a1, ArcOfParabola& a2, int tagId, bool driving) { - Constraint *constr = new ConstraintEqualFocalDistance(&a1,&a2); + Constraint* constr = new ConstraintEqualFocalDistance(&a1, &a2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintEqualRadius(Circle &c1, Arc &a2, int tagId, bool driving) +int System::addConstraintEqualRadius(Circle& c1, Arc& a2, int tagId, bool driving) { return addConstraintEqual(c1.rad, a2.rad, tagId, driving); } -int System::addConstraintEqualRadius(Arc &a1, Arc &a2, int tagId, bool driving) +int System::addConstraintEqualRadius(Arc& a1, Arc& a2, int tagId, bool driving) { return addConstraintEqual(a1.rad, a2.rad, tagId, driving); } -int System::addConstraintP2PSymmetric(Point &p1, Point &p2, Line &l, int tagId, bool driving) +int System::addConstraintP2PSymmetric(Point& p1, Point& p2, Line& l, int tagId, bool driving) { addConstraintPerpendicular(p1, p2, l.p1, l.p2, tagId, driving); return addConstraintMidpointOnLine(p1, p2, l.p1, l.p2, tagId, driving); } -int System::addConstraintP2PSymmetric(Point &p1, Point &p2, Point &p, int tagId, bool driving) +int System::addConstraintP2PSymmetric(Point& p1, Point& p2, Point& p, int tagId, bool driving) { addConstraintPointOnPerpBisector(p, p1, p2, tagId, driving); return addConstraintPointOnLine(p, p1, p2, tagId, driving); } -int System::addConstraintSnellsLaw(Curve &ray1, Curve &ray2, - Curve &boundary, Point p, - double *n1, double *n2, - bool flipn1, bool flipn2, - int tagId, bool driving) +int System::addConstraintSnellsLaw(Curve& ray1, Curve& ray2, Curve& boundary, Point p, double* n1, + double* n2, bool flipn1, bool flipn2, int tagId, bool driving) { - Constraint *constr = new ConstraintSnell(ray1,ray2,boundary,p,n1,n2,flipn1,flipn2); + Constraint* constr = new ConstraintSnell(ray1, ray2, boundary, p, n1, n2, flipn1, flipn2); constr->setTag(tagId); constr->setDriving(driving); return addConstraint(constr); @@ -1547,29 +1546,28 @@ void System::calculateNormalAtPoint(const Curve& crv, const Point& p, double& rt double System::calculateConstraintErrorByTag(int tagId) { - int cnt = 0; //how many constraints have been accumulated - double sqErr = 0.0; //accumulator of squared errors - double err = 0.0;//last computed signed error value + int cnt = 0; // how many constraints have been accumulated + double sqErr = 0.0;// accumulator of squared errors + double err = 0.0; // last computed signed error value - for (std::vector::const_iterator - constr=clist.begin(); constr != clist.end(); ++constr) { - if ((*constr)->getTag() == tagId){ + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + if ((*constr)->getTag() == tagId) { err = (*constr)->error(); - sqErr += err*err; + sqErr += err * err; cnt++; }; } switch (cnt) { - case 0: //constraint not found! + case 0:// constraint not found! return std::numeric_limits::quiet_NaN(); - break; + break; case 1: return err; - break; + break; default: - return sqrt(sqErr/(double)cnt); + return sqrt(sqErr / (double)cnt); } - } void System::rescaleConstraint(int id, double coeff) @@ -1580,16 +1578,16 @@ void System::rescaleConstraint(int id, double coeff) clist[id]->rescale(coeff); } -void System::declareUnknowns(VEC_pD ¶ms) +void System::declareUnknowns(VEC_pD& params) { plist = params; pIndex.clear(); - for (int i=0; i < int(plist.size()); ++i) + for (int i = 0; i < int(plist.size()); ++i) pIndex[plist[i]] = i; hasUnknowns = true; } -void System::declareDrivenParams(VEC_pD ¶ms) +void System::declareDrivenParams(VEC_pD& params) { pdrivenlist = params; } @@ -1620,12 +1618,12 @@ void System::initSolution(Algorithm alg) if (!hasDiagnosis) return; } - std::vector clistR; + std::vector clistR; if (!redundant.empty()) { for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); ++constr) { if (redundant.count(*constr) == 0) - clistR.push_back(*constr); + clistR.push_back(*constr); } } else { @@ -1634,15 +1632,14 @@ void System::initSolution(Algorithm alg) // partitioning into decoupled components Graph g; - for (int i=0; i < int(plist.size() + clistR.size()); i++) + for (int i = 0; i < int(plist.size() + clistR.size()); i++) boost::add_vertex(g); int cvtid = int(plist.size()); - for (std::vector::const_iterator constr=clistR.begin(); - constr != clistR.end(); ++constr, cvtid++) { - VEC_pD &cparams = c2p[*constr]; - for (VEC_pD::const_iterator param=cparams.begin(); - param != cparams.end(); ++param) { + for (std::vector::const_iterator constr = clistR.begin(); constr != clistR.end(); + ++constr, cvtid++) { + VEC_pD& cparams = c2p[*constr]; + for (VEC_pD::const_iterator param = cparams.begin(); param != cparams.end(); ++param) { MAP_pD_I::const_iterator it = pIndex.find(*param); if (it != pIndex.end()) boost::add_edge(cvtid, it->second, g); @@ -1655,63 +1652,65 @@ void System::initSolution(Algorithm alg) componentsSize = boost::connected_components(g, &components[0]); // identification of equality constraints and parameter reduction - std::set reducedConstrs; // constraints that will be eliminated through reduction - reductionmaps.clear(); // destroy any maps - reductionmaps.resize(componentsSize); // create empty maps to be filled in + std::set reducedConstrs;// constraints that will be eliminated through reduction + reductionmaps.clear(); // destroy any maps + reductionmaps.resize(componentsSize);// create empty maps to be filled in { - VEC_pD reducedParams=plist; + VEC_pD reducedParams = plist; - for (std::vector::const_iterator constr=clistR.begin(); - constr != clistR.end(); ++constr) { + for (std::vector::const_iterator constr = clistR.begin(); + constr != clistR.end(); + ++constr) { if ((*constr)->getTag() >= 0 && (*constr)->getTypeId() == Equal) { - MAP_pD_I::const_iterator it1,it2; + MAP_pD_I::const_iterator it1, it2; it1 = pIndex.find((*constr)->params()[0]); it2 = pIndex.find((*constr)->params()[1]); if (it1 != pIndex.end() && it2 != pIndex.end()) { reducedConstrs.insert(*constr); - double *p_kept = reducedParams[it1->second]; - double *p_replaced = reducedParams[it2->second]; - for (int i=0; i < int(plist.size()); ++i) { - if (reducedParams[i] == p_replaced) - reducedParams[i] = p_kept; + double* p_kept = reducedParams[it1->second]; + double* p_replaced = reducedParams[it2->second]; + for (int i = 0; i < int(plist.size()); ++i) { + if (reducedParams[i] == p_replaced) + reducedParams[i] = p_kept; } } } } - for (int i=0; i < int(plist.size()); ++i) + for (int i = 0; i < int(plist.size()); ++i) if (plist[i] != reducedParams[i]) { int cid = components[i]; reductionmaps[cid][plist[i]] = reducedParams[i]; } } - clists.clear(); // destroy any lists - clists.resize(componentsSize); // create empty lists to be filled in + clists.clear(); // destroy any lists + clists.resize(componentsSize);// create empty lists to be filled in int i = int(plist.size()); - for (std::vector::const_iterator constr=clistR.begin(); - constr != clistR.end(); ++constr, i++) { + for (std::vector::const_iterator constr = clistR.begin(); constr != clistR.end(); + ++constr, i++) { if (reducedConstrs.count(*constr) == 0) { int cid = components[i]; clists[cid].push_back(*constr); } } - plists.clear(); // destroy any lists - plists.resize(componentsSize); // create empty lists to be filled in - for (int i=0; i < int(plist.size()); ++i) { + plists.clear(); // destroy any lists + plists.resize(componentsSize);// create empty lists to be filled in + for (int i = 0; i < int(plist.size()); ++i) { int cid = components[i]; plists[cid].push_back(plist[i]); } // calculates subSystems and subSystemsAux from clists, plists and reductionmaps clearSubSystems(); - for (std::size_t cid=0; cid < clists.size(); cid++) { - std::vector clist0, clist1; - for (std::vector::const_iterator constr=clists[cid].begin(); - constr != clists[cid].end(); ++constr) { + for (std::size_t cid = 0; cid < clists.size(); cid++) { + std::vector clist0, clist1; + for (std::vector::const_iterator constr = clists[cid].begin(); + constr != clists[cid].end(); + ++constr) { if ((*constr)->getTag() >= 0) clist0.push_back(*constr); - else // move or distance from reference constraints + else// move or distance from reference constraints clist1.push_back(*constr); } @@ -1730,22 +1729,21 @@ void System::setReference() { reference.clear(); reference.reserve(plist.size()); - for (VEC_pD::const_iterator param=plist.begin(); - param != plist.end(); ++param) + for (VEC_pD::const_iterator param = plist.begin(); param != plist.end(); ++param) reference.push_back(**param); } void System::resetToReference() { if (reference.size() == plist.size()) { - VEC_D::const_iterator ref=reference.begin(); - VEC_pD::iterator param=plist.begin(); + VEC_D::const_iterator ref = reference.begin(); + VEC_pD::iterator param = plist.begin(); for (; ref != reference.end(); ++ref, ++param) **param = *ref; } } -int System::solve(VEC_pD ¶ms, bool isFine, Algorithm alg, bool isRedundantsolving) +int System::solve(VEC_pD& params, bool isFine, Algorithm alg, bool isRedundantsolving) { declareUnknowns(params); initSolution(); @@ -1761,27 +1759,28 @@ int System::solve(bool isFine, Algorithm alg, bool isRedundantsolving) // return success by default in order to permit coincidence constraints to be applied // even if no other system has to be solved int res = Success; - for (int cid=0; cid < int(subSystems.size()); cid++) { + for (int cid = 0; cid < int(subSystems.size()); cid++) { if ((subSystems[cid] || subSystemsAux[cid]) && !isReset) { - resetToReference(); - isReset = true; + resetToReference(); + isReset = true; } if (subSystems[cid] && subSystemsAux[cid]) - res = std::max(res, - solve(subSystems[cid], subSystemsAux[cid], isFine, isRedundantsolving)); + res = std::max(res, + solve(subSystems[cid], subSystemsAux[cid], isFine, isRedundantsolving)); else if (subSystems[cid]) res = std::max(res, solve(subSystems[cid], isFine, alg, isRedundantsolving)); else if (subSystemsAux[cid]) res = std::max(res, solve(subSystemsAux[cid], isFine, alg, isRedundantsolving)); } if (res == Success) { - for (std::set::const_iterator constr=redundant.begin(); - constr != redundant.end(); ++constr){ - //DeepSOIC: there used to be a comparison of signed error value to - //convergence, which makes no sense. Potentially I fixed bug, and - //chances are low I've broken anything. + for (std::set::const_iterator constr = redundant.begin(); + constr != redundant.end(); + ++constr) { + // DeepSOIC: there used to be a comparison of signed error value to + // convergence, which makes no sense. Potentially I fixed bug, and + // chances are low I've broken anything. double err = (*constr)->error(); - if (err*err > (isRedundantsolving?convergenceRedundant:convergence)) { + if (err * err > (isRedundantsolving ? convergenceRedundant : convergence)) { res = Converged; return res; } @@ -1790,7 +1789,7 @@ int System::solve(bool isFine, Algorithm alg, bool isRedundantsolving) return res; } -int System::solve(SubSystem *subsys, bool isFine, Algorithm alg, bool isRedundantsolving) +int System::solve(SubSystem* subsys, bool isFine, Algorithm alg, bool isRedundantsolving) { if (alg == BFGS) return solve_BFGS(subsys, isFine, isRedundantsolving); @@ -1802,11 +1801,11 @@ int System::solve(SubSystem *subsys, bool isFine, Algorithm alg, bool isRedundan return Failed; } -int System::solve_BFGS(SubSystem *subsys, bool /*isFine*/, bool isRedundantsolving) +int System::solve_BFGS(SubSystem* subsys, bool /*isFine*/, bool isRedundantsolving) { - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ extractSubsystem(subsys, isRedundantsolving); - #endif +#endif int xsize = subsys->pSize(); if (xsize == 0) @@ -1833,46 +1832,44 @@ int System::solve_BFGS(SubSystem *subsys, bool /*isFine*/, bool isRedundantsolvi h = x; subsys->getParams(x); - h = x - h; // = x - xold + h = x - h;// = x - xold - //double convergence = isFine ? convergence : XconvergenceRough; - int maxIterNumber = (isRedundantsolving? - (sketchSizeMultiplierRedundant?maxIterRedundant * xsize:maxIterRedundant): - (sketchSizeMultiplier?maxIter * xsize:maxIter)); + // double convergence = isFine ? convergence : XconvergenceRough; + int maxIterNumber = + (isRedundantsolving + ? (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant) + : (sketchSizeMultiplier ? maxIter * xsize : maxIter)); - if(debugMode==IterationLevel) { + if (debugMode == IterationLevel) { std::stringstream stream; - stream << "BFGS: convergence: " << (isRedundantsolving?convergenceRedundant:convergence) - << ", xsize: " << xsize - << ", maxIter: " << maxIterNumber << "\n"; + stream << "BFGS: convergence: " << (isRedundantsolving ? convergenceRedundant : convergence) + << ", xsize: " << xsize << ", maxIter: " << maxIterNumber << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); } - double divergingLim = 1e6*err + 1e12; + double divergingLim = 1e6 * err + 1e12; double h_norm; - for (int iter=1; iter < maxIterNumber; iter++) { + for (int iter = 1; iter < maxIterNumber; iter++) { h_norm = h.norm(); - if (h_norm <= (isRedundantsolving?convergenceRedundant:convergence) || err <= smallF){ - if(debugMode==IterationLevel) { + if (h_norm <= (isRedundantsolving ? convergenceRedundant : convergence) || err <= smallF) { + if (debugMode == IterationLevel) { std::stringstream stream; - stream << "BFGS Converged!!: " - << ", err: " << err - << ", h_norm: " << h_norm << "\n"; + stream << "BFGS Converged!!: " + << ", err: " << err << ", h_norm: " << h_norm << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); } break; } - if (err > divergingLim || err != err) { // check for diverging and NaN - if(debugMode==IterationLevel) { + if (err > divergingLim || err != err) {// check for diverging and NaN + if (debugMode == IterationLevel) { std::stringstream stream; - stream << "BFGS Failed: Diverging!!: " - << ", err: " << err - << ", divergingLim: " << divergingLim << "\n"; + stream << "BFGS Failed: Diverging!!: " + << ", err: " << err << ", divergingLim: " << divergingLim << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); @@ -1882,10 +1879,10 @@ int System::solve_BFGS(SubSystem *subsys, bool /*isFine*/, bool isRedundantsolvi y = grad; subsys->calcGrad(grad); - y = grad - y; // = grad - gradold + y = grad - y;// = grad - gradold double hty = h.dot(y); - //make sure that hty is never 0 + // make sure that hty is never 0 if (hty == 0) hty = .0000000001; @@ -1893,9 +1890,9 @@ int System::solve_BFGS(SubSystem *subsys, bool /*isFine*/, bool isRedundantsolvi double ytDy = y.dot(Dy); - //Now calculate the BFGS update on D - D += (1.+ytDy/hty)/hty * h * h.transpose(); - D -= 1./hty * (h * Dy.transpose() + Dy * h.transpose()); + // Now calculate the BFGS update on D + D += (1. + ytDy / hty) / hty * h * h.transpose(); + D -= 1. / hty * (h * Dy.transpose() + Dy * h.transpose()); xdir = -D * grad; lineSearch(subsys, xdir); @@ -1903,13 +1900,12 @@ int System::solve_BFGS(SubSystem *subsys, bool /*isFine*/, bool isRedundantsolvi h = x; subsys->getParams(x); - h = x - h; // = x - xold + h = x - h;// = x - xold - if(debugMode==IterationLevel) { + if (debugMode == IterationLevel) { std::stringstream stream; - stream << "BFGS, Iteration: " << iter - << ", err: " << err - << ", h_norm: " << h_norm << "\n"; + stream << "BFGS, Iteration: " << iter << ", err: " << err << ", h_norm: " << h_norm + << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); @@ -1920,7 +1916,7 @@ int System::solve_BFGS(SubSystem *subsys, bool /*isFine*/, bool isRedundantsolvi if (err <= smallF) return Success; - if (h.norm() <= (isRedundantsolving?convergenceRedundant:convergence)) + if (h.norm() <= (isRedundantsolving ? convergenceRedundant : convergence)) return Converged; return Failed; } @@ -1947,55 +1943,54 @@ int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) subsys->getParams(x); subsys->calcResidual(e); - e*=-1; + e *= -1; - int maxIterNumber = (isRedundantsolving? - (sketchSizeMultiplierRedundant?maxIterRedundant * xsize:maxIterRedundant): - (sketchSizeMultiplier?maxIter * xsize:maxIter)); + int maxIterNumber = + (isRedundantsolving + ? (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant) + : (sketchSizeMultiplier ? maxIter * xsize : maxIter)); - double divergingLim = 1e6*e.squaredNorm() + 1e12; + double divergingLim = 1e6 * e.squaredNorm() + 1e12; - double eps=(isRedundantsolving?LM_epsRedundant:LM_eps); - double eps1=(isRedundantsolving?LM_eps1Redundant:LM_eps1); - double tau=(isRedundantsolving?LM_tauRedundant:LM_tau); + double eps = (isRedundantsolving ? LM_epsRedundant : LM_eps); + double eps1 = (isRedundantsolving ? LM_eps1Redundant : LM_eps1); + double tau = (isRedundantsolving ? LM_tauRedundant : LM_tau); - if(debugMode==IterationLevel) { + if (debugMode == IterationLevel) { std::stringstream stream; - stream << "LM: eps: " << eps - << ", eps1: " << eps1 - << ", tau: " << tau - << ", convergence: " << (isRedundantsolving?convergenceRedundant:convergence) - << ", xsize: " << xsize - << ", maxIter: " << maxIterNumber << "\n"; + stream << "LM: eps: " << eps << ", eps1: " << eps1 << ", tau: " << tau + << ", convergence: " << (isRedundantsolving ? convergenceRedundant : convergence) + << ", xsize: " << xsize << ", maxIter: " << maxIterNumber << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); } - double nu=2, mu=0; - int iter=0, stop=0; - for (iter=0; iter < maxIterNumber && !stop; ++iter) { + double nu = 2, mu = 0; + int iter = 0, stop = 0; + for (iter = 0; iter < maxIterNumber && !stop; ++iter) { // check error - double err=e.squaredNorm(); - if (err <= eps*eps) { // error is small, Success + double err = e.squaredNorm(); + if (err <= eps * eps) {// error is small, Success stop = 1; break; } - else if (err > divergingLim || err != err) { // check for diverging and NaN + else if (err > divergingLim || err != err) {// check for diverging and NaN stop = 6; break; } // J^T J, J^T e - subsys->calcJacobi(J);; + subsys->calcJacobi(J); + ; - A = J.transpose()*J; - g = J.transpose()*e; + A = J.transpose() * J; + g = J.transpose() * e; // Compute ||J^T e||_inf double g_inf = g.lpNorm(); - diag_A = A.diagonal(); // save diagonal entries so that augmentation can be later canceled + diag_A = A.diagonal();// save diagonal entries so that augmentation can be later canceled // check for convergence if (g_inf <= eps1) { @@ -2009,15 +2004,15 @@ int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) double h_norm; // determine increment using adaptive damping - int k=0; + int k = 0; while (k < 50) { // augment normal equations A = A+uI - for (int i=0; i < xsize; ++i) - A(i,i) += mu; + for (int i = 0; i < xsize; ++i) + A(i, i) += mu; - //solve augmented functions A*h=-g + // solve augmented functions A*h=-g h = A.fullPivLu().solve(g); - double rel_error = (A*h - g).norm() / g.norm(); + double rel_error = (A * h - g).norm() / g.norm(); // check if solving works if (rel_error < 1e-5) { @@ -2031,11 +2026,12 @@ int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) x_new = x + h; h_norm = h.squaredNorm(); - if (h_norm <= eps1*eps1*x.norm()) { // relative change in p is small, stop + if (h_norm <= eps1 * eps1 * x.norm()) {// relative change in p is small, stop stop = 3; break; } - else if (h_norm >= (x.norm()+eps1)/(DBL_EPSILON*DBL_EPSILON)) { // almost singular + else if (h_norm + >= (x.norm() + eps1) / (DBL_EPSILON * DBL_EPSILON)) {// almost singular stop = 4; break; } @@ -2045,12 +2041,12 @@ int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) e_new *= -1; double dF = e.squaredNorm() - e_new.squaredNorm(); - double dL = h.dot(mu*h+g); + double dL = h.dot(mu * h + g); - if (dF>0. && dL>0.) { // reduction in error, increment is accepted - double tmp=2*dF/dL-1.; - mu *= std::max(1./3., 1.-tmp*tmp*tmp); - nu=2; + if (dF > 0. && dL > 0.) {// reduction in error, increment is accepted + double tmp = 2 * dF / dL - 1.; + mu *= std::max(1. / 3., 1. - tmp * tmp * tmp); + nu = 2; // update par's estimate x = x_new; @@ -2062,10 +2058,10 @@ int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) // if this point is reached, either the linear system could not be solved or // the error did not reduce; in any case, the increment must be rejected - mu*=nu; - nu*=2.0; - for (int i=0; i < xsize; ++i) // restore diagonal J^T J entries - A(i,i) = diag_A(i); + mu *= nu; + nu *= 2.0; + for (int i = 0; i < xsize; ++i)// restore diagonal J^T J entries + A(i, i) = diag_A(i); k++; } @@ -2074,13 +2070,11 @@ int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) break; } - if(debugMode==IterationLevel) { + if (debugMode == IterationLevel) { std::stringstream stream; // Iteration: 1, residual: 1e-3, tolg: 1e-5, tolx: 1e-3 - stream << "LM, Iteration: " << iter - << ", err(eps): " << err - << ", g_inf(eps1): " << g_inf - << ", h_norm: " << h_norm << "\n"; + stream << "LM, Iteration: " << iter << ", err(eps): " << err + << ", g_inf(eps1): " << g_inf << ", h_norm: " << h_norm << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); @@ -2102,9 +2096,9 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) extractSubsystem(subsys, isRedundantsolving); #endif - double tolg=(isRedundantsolving?DL_tolgRedundant:DL_tolg); - double tolx=(isRedundantsolving?DL_tolxRedundant:DL_tolx); - double tolf=(isRedundantsolving?DL_tolfRedundant:DL_tolf); + double tolg = (isRedundantsolving ? DL_tolgRedundant : DL_tolg); + double tolx = (isRedundantsolving ? DL_tolxRedundant : DL_tolx); + double tolf = (isRedundantsolving ? DL_tolfRedundant : DL_tolf); int xsize = subsys->pSize(); int csize = subsys->cSize(); @@ -2112,9 +2106,10 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) if (xsize == 0) return Success; - int maxIterNumber = (isRedundantsolving? - (sketchSizeMultiplierRedundant?maxIterRedundant * xsize:maxIterRedundant): - (sketchSizeMultiplier?maxIter * xsize:maxIter)); + int maxIterNumber = + (isRedundantsolving + ? (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant) + : (sketchSizeMultiplier ? maxIter * xsize : maxIter)); if (debugMode == IterationLevel) { std::stringstream stream; @@ -2144,82 +2139,82 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) subsys->calcResidual(fx, err); subsys->calcJacobi(Jx); - g = Jx.transpose()*(-fx); + g = Jx.transpose() * (-fx); // get the infinity norm fx_inf and g_inf double g_inf = g.lpNorm(); double fx_inf = fx.lpNorm(); - double divergingLim = 1e6*err + 1e12; + double divergingLim = 1e6 * err + 1e12; - double delta=0.1; - double alpha=0.; - double nu=2.; - int iter=0, stop=0, reduce=0; + double delta = 0.1; + double alpha = 0.; + double nu = 2.; + int iter = 0, stop = 0, reduce = 0; while (!stop) { // check if finished - if (fx_inf <= tolf) // Success + if (fx_inf <= tolf)// Success stop = 1; else if (g_inf <= tolg) stop = 2; - else if (delta <= tolx*(tolx + x.norm())) + else if (delta <= tolx * (tolx + x.norm())) stop = 2; else if (iter >= maxIterNumber) stop = 4; - else if (err > divergingLim || err != err) { // check for diverging and NaN + else if (err > divergingLim || err != err) {// check for diverging and NaN stop = 6; } else { // get the steepest descent direction - alpha = g.squaredNorm()/(Jx*g).squaredNorm(); - h_sd = alpha*g; + alpha = g.squaredNorm() / (Jx * g).squaredNorm(); + h_sd = alpha * g; // get the gauss-newton step // http://forum.freecad.org/viewtopic.php?f=10&t=12769&start=50#p106220 // https://forum.kde.org/viewtopic.php?f=74&t=129439#p346104 - switch (dogLegGaussStep){ + switch (dogLegGaussStep) { case FullPivLU: h_gn = Jx.fullPivLu().solve(-fx); break; case LeastNormFullPivLU: - h_gn = Jx.adjoint()*(Jx*Jx.adjoint()).fullPivLu().solve(-fx); + h_gn = Jx.adjoint() * (Jx * Jx.adjoint()).fullPivLu().solve(-fx); break; case LeastNormLdlt: - h_gn = Jx.adjoint()*(Jx*Jx.adjoint()).ldlt().solve(-fx); + h_gn = Jx.adjoint() * (Jx * Jx.adjoint()).ldlt().solve(-fx); break; } - double rel_error = (Jx*h_gn + fx).norm() / fx.norm(); + double rel_error = (Jx * h_gn + fx).norm() / fx.norm(); if (rel_error > 1e15) break; // compute the dogleg step if (h_gn.norm() < delta) { h_dl = h_gn; - if (h_dl.norm() <= tolx*(tolx + x.norm())) { + if (h_dl.norm() <= tolx * (tolx + x.norm())) { stop = 5; break; } } - else if (alpha*g.norm() >= delta) { - h_dl = (delta/(alpha*g.norm()))*h_sd; + else if (alpha * g.norm() >= delta) { + h_dl = (delta / (alpha * g.norm())) * h_sd; } else { - //compute beta + // compute beta double beta = 0; Eigen::VectorXd b = h_gn - h_sd; - double bb = (b.transpose()*b).norm(); - double gb = (h_sd.transpose()*b).norm(); - double c = (delta + h_sd.norm())*(delta - h_sd.norm()); + double bb = (b.transpose() * b).norm(); + double gb = (h_sd.transpose() * b).norm(); + double c = (delta + h_sd.norm()) * (delta - h_sd.norm()); if (gb > 0) beta = c / (gb + sqrt(gb * gb + c * bb)); else - beta = (sqrt(gb * gb + c * bb) - gb)/bb; + beta = (sqrt(gb * gb + c * bb) - gb) / bb; // and update h_dl and dL with beta - h_dl = h_sd + beta*b; + h_dl = h_sd + beta * b; } } @@ -2227,11 +2222,11 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) if (stop) break; -// it didn't work in some tests -// // restrict h_dl according to maxStep -// double scale = subsys->maxStep(h_dl); -// if (scale < 1.) -// h_dl *= scale; + // it didn't work in some tests + // // restrict h_dl according to maxStep + // double scale = subsys->maxStep(h_dl); + // if (scale < 1.) + // h_dl *= scale; // get the new values double err_new; @@ -2241,17 +2236,17 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) subsys->calcJacobi(Jx_new); // calculate the linear model and the update ratio - double dL = err - 0.5*(fx + Jx*h_dl).squaredNorm(); + double dL = err - 0.5 * (fx + Jx * h_dl).squaredNorm(); double dF = err - err_new; - double rho = dL/dF; + double rho = dL / dF; if (dF > 0 && dL > 0) { - x = x_new; + x = x_new; Jx = Jx_new; fx = fx_new; err = err_new; - g = Jx.transpose()*(-fx); + g = Jx.transpose() * (-fx); // get infinity norms g_inf = g.lpNorm(); @@ -2261,27 +2256,25 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) rho = -1; // update delta - if (fabs(rho-1.) < 0.2 && h_dl.norm() > delta/3. && reduce <= 0) { - delta = 3*delta; + if (fabs(rho - 1.) < 0.2 && h_dl.norm() > delta / 3. && reduce <= 0) { + delta = 3 * delta; nu = 2; reduce = 0; } else if (rho < 0.25) { - delta = delta/nu; - nu = 2*nu; + delta = delta / nu; + nu = 2 * nu; reduce = 2; } else reduce--; - if(debugMode==IterationLevel) { + if (debugMode == IterationLevel) { std::stringstream stream; // Iteration: 1, residual: 1e-3, tolg: 1e-5, tolx: 1e-3 - stream << "DL, Iteration: " << iter - << ", fx_inf(tolf): " << fx_inf - << ", g_inf(tolg): " << g_inf - << ", delta(f(tolx)): " << delta - << ", err(divergingLim): " << err << "\n"; + stream << "DL, Iteration: " << iter << ", fx_inf(tolf): " << fx_inf + << ", g_inf(tolg): " << g_inf << ", delta(f(tolx)): " << delta + << ", err(divergingLim): " << err << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); @@ -2293,9 +2286,9 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) subsys->revertParams(); - if(debugMode==IterationLevel) { + if (debugMode == IterationLevel) { std::stringstream stream; - stream << "DL: stopcode: " << stop << ((stop == 1) ? ", Success" : ", Failed") << "\n"; + stream << "DL: stopcode: " << stop << ((stop == 1) ? ", Success" : ", Failed") << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); @@ -2326,8 +2319,8 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) subsystemfile << "GCS::VEC_pD plist_;" << std::endl; // all SYSTEM params subsystemfile << "std::vector clist_;" << std::endl;// SUBSYSTEM constraints subsystemfile << "GCS::VEC_pD plistsub_;" << std::endl; // all SUBSYSTEM params - subsystemfile << "GCS::VEC_pD clist_params_;" - << std::endl;// constraint params not within SYSTEM params + // constraint params not within SYSTEM params + subsystemfile << "GCS::VEC_pD clist_params_;" << std::endl; // these are all the parameters, including those not in the subsystem to // which constraints in the subsystem may make reference. @@ -2367,14 +2360,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -2386,14 +2379,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -2420,14 +2413,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -2439,14 +2432,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -2458,14 +2451,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -2498,14 +2491,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -2517,14 +2510,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -2536,14 +2529,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -2555,14 +2548,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -2574,14 +2567,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -2630,14 +2623,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -2649,14 +2642,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -2668,14 +2661,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -2687,14 +2680,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -2706,14 +2699,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -2766,14 +2759,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -2785,14 +2778,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -2804,14 +2797,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -2823,14 +2816,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -2842,14 +2835,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -2861,14 +2854,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i6 == plist.size()) { if (ni6 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[5]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[5]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] + << std::endl; + icp++; } npb6 = true; } @@ -2880,14 +2873,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i7 == plist.size()) { if (ni7 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[6]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[6]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] + << std::endl; + icp++; } npb7 = true; } @@ -2945,14 +2938,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -2964,14 +2957,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -2983,14 +2976,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -3002,14 +2995,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -3021,14 +3014,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -3040,14 +3033,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i6 == plist.size()) { if (ni6 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[5]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[5]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] + << std::endl; + icp++; } npb6 = true; } @@ -3101,14 +3094,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -3120,14 +3113,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -3139,14 +3132,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -3158,14 +3151,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -3177,14 +3170,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -3196,14 +3189,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i6 == plist.size()) { if (ni6 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[5]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[5]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] + << std::endl; + icp++; } npb6 = true; } @@ -3261,14 +3254,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -3280,14 +3273,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -3299,14 +3292,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -3318,14 +3311,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -3337,14 +3330,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -3356,14 +3349,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i6 == plist.size()) { if (ni6 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[5]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[5]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] + << std::endl; + icp++; } npb6 = true; } @@ -3375,14 +3368,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i7 == plist.size()) { if (ni7 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[6]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[6]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] + << std::endl; + icp++; } npb7 = true; } @@ -3394,14 +3387,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i8 == plist.size()) { if (ni8 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[7]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[7] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[7]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[7] + << std::endl; + icp++; } npb8 = true; } @@ -3466,14 +3459,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -3485,14 +3478,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -3504,14 +3497,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -3523,14 +3516,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -3542,14 +3535,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -3561,14 +3554,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i6 == plist.size()) { if (ni6 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[5]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[5]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] + << std::endl; + icp++; } npb6 = true; } @@ -3580,14 +3573,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i7 == plist.size()) { if (ni7 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[6]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[6]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] + << std::endl; + icp++; } npb7 = true; } @@ -3599,14 +3592,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i8 == plist.size()) { if (ni8 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[7]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[7] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[7]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[7] + << std::endl; + icp++; } npb8 = true; } @@ -3673,14 +3666,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -3692,14 +3685,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -3711,14 +3704,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -3730,14 +3723,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -3749,14 +3742,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -3768,14 +3761,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i6 == plist.size()) { if (ni6 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[5]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[5]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] + << std::endl; + icp++; } npb6 = true; } @@ -3787,14 +3780,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i7 == plist.size()) { if (ni7 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[6]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[6]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] + << std::endl; + icp++; } npb7 = true; } @@ -3806,14 +3799,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i8 == plist.size()) { if (ni8 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[7]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[7] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[7]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[7] + << std::endl; + icp++; } npb8 = true; } @@ -3825,14 +3818,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i9 == plist.size()) { if (ni9 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[8]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[8]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[8] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[8]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[8]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[8] + << std::endl; + icp++; } npb9 = true; } @@ -3900,14 +3893,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -3919,14 +3912,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -3938,14 +3931,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -3957,14 +3950,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -3976,14 +3969,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -3995,14 +3988,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i6 == plist.size()) { if (ni6 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[5]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[5]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] + << std::endl; + icp++; } npb6 = true; } @@ -4014,14 +4007,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i7 == plist.size()) { if (ni7 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[6]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[6]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] + << std::endl; + icp++; } npb7 = true; } @@ -4033,14 +4026,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i8 == plist.size()) { if (ni8 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[7]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[7] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[7]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[7] + << std::endl; + icp++; } npb8 = true; } @@ -4101,14 +4094,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -4120,14 +4113,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -4139,14 +4132,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -4158,14 +4151,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -4177,14 +4170,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -4196,14 +4189,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i6 == plist.size()) { if (ni6 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[5]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[5]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] + << std::endl; + icp++; } npb6 = true; } @@ -4263,14 +4256,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i1 == plist.size()) { if (ni1 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[0]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[0]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[0] + << std::endl; + icp++; } npb1 = true; } @@ -4282,14 +4275,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i2 == plist.size()) { if (ni2 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[1]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[1]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[1] + << std::endl; + icp++; } npb2 = true; } @@ -4301,14 +4294,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i3 == plist.size()) { if (ni3 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[2]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[2]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[2] + << std::endl; + icp++; } npb3 = true; } @@ -4320,14 +4313,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i4 == plist.size()) { if (ni4 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[3]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[3]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[3] + << std::endl; + icp++; } npb4 = true; } @@ -4339,14 +4332,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i5 == plist.size()) { if (ni5 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[4]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[4]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[4] + << std::endl; + icp++; } npb5 = true; } @@ -4358,14 +4351,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i6 == plist.size()) { if (ni6 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[5]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[5]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[5] + << std::endl; + icp++; } npb6 = true; } @@ -4377,14 +4370,14 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) if (i7 == plist.size()) { if (ni7 == clist_params_.size()) { - subsystemfile - << "// Address not in System params...rebuilding into clist_params_" - << std::endl; - clist_params_.push_back((*it)->pvec[6]); - subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) - << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] - << std::endl; - icp++; + subsystemfile + << "// Address not in System params...rebuilding into clist_params_" + << std::endl; + clist_params_.push_back((*it)->pvec[6]); + subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6]) + << ")); // " << icp << " address: " << (void*)(*it)->pvec[6] + << std::endl; + icp++; } npb7 = true; } @@ -4437,31 +4430,31 @@ void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) // The following solver variant solves a system compound of two subsystems // treating the first of them as of higher priority than the second -int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool /*isFine*/, bool isRedundantsolving) +int System::solve(SubSystem* subsysA, SubSystem* subsysB, bool /*isFine*/, bool isRedundantsolving) { int xsizeA = subsysA->pSize(); int xsizeB = subsysB->pSize(); int csizeA = subsysA->cSize(); - VEC_pD plistAB(xsizeA+xsizeB); + VEC_pD plistAB(xsizeA + xsizeB); { VEC_pD plistA, plistB; subsysA->getParamList(plistA); subsysB->getParamList(plistB); - std::sort(plistA.begin(),plistA.end()); - std::sort(plistB.begin(),plistB.end()); + std::sort(plistA.begin(), plistA.end()); + std::sort(plistB.begin(), plistB.end()); VEC_pD::const_iterator it; - it = std::set_union(plistA.begin(),plistA.end(), - plistB.begin(),plistB.end(),plistAB.begin()); - plistAB.resize(it-plistAB.begin()); + it = std::set_union( + plistA.begin(), plistA.end(), plistB.begin(), plistB.end(), plistAB.begin()); + plistAB.resize(it - plistAB.begin()); } int xsize = plistAB.size(); Eigen::MatrixXd B = Eigen::MatrixXd::Identity(xsize, xsize); Eigen::MatrixXd JA(csizeA, xsize); - Eigen::MatrixXd Y,Z; + Eigen::MatrixXd Y, Z; Eigen::VectorXd resA(csizeA); Eigen::VectorXd lambda(csizeA), lambda0(csizeA), lambdadir(csizeA); @@ -4475,24 +4468,25 @@ int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool /*isFine*/, bool subsysA->redirectParams(); subsysB->redirectParams(); - subsysB->getParams(plistAB,x); - subsysA->getParams(plistAB,x); - subsysB->setParams(plistAB,x); // just to ensure that A and B are synchronized + subsysB->getParams(plistAB, x); + subsysA->getParams(plistAB, x); + subsysB->setParams(plistAB, x);// just to ensure that A and B are synchronized - subsysB->calcGrad(plistAB,grad); - subsysA->calcJacobi(plistAB,JA); + subsysB->calcGrad(plistAB, grad); + subsysA->calcJacobi(plistAB, JA); subsysA->calcResidual(resA); - //double convergence = isFine ? XconvergenceFine : XconvergenceRough; - int maxIterNumber = (isRedundantsolving? - (sketchSizeMultiplierRedundant?maxIterRedundant * xsize:maxIterRedundant): - (sketchSizeMultiplier?maxIter * xsize:maxIter)); + // double convergence = isFine ? XconvergenceFine : XconvergenceRough; + int maxIterNumber = + (isRedundantsolving + ? (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant) + : (sketchSizeMultiplier ? maxIter * xsize : maxIter)); - double divergingLim = 1e6*subsysA->error() + 1e12; + double divergingLim = 1e6 * subsysA->error() + 1e12; double mu = 0; lambda.setZero(); - for (int iter=1; iter < maxIterNumber; iter++) { + for (int iter = 1; iter < maxIterNumber; iter++) { int status = qp_eq(B, grad, JA, resA, xdir, Y, Z); if (status) break; @@ -4504,11 +4498,11 @@ int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool /*isFine*/, bool // line search { - double eta=0.25; - double tau=0.5; - double rho=0.5; - double alpha=1; - alpha = std::min(alpha, subsysA->maxStep(plistAB,xdir)); + double eta = 0.25; + double tau = 0.5; + double rho = 0.5; + double alpha = 1; + alpha = std::min(alpha, subsysA->maxStep(plistAB, xdir)); // From the book "Numerical Optimization - Jorge Nocedal, Stephen J. Wright". // See https://forum.freecad.org/viewtopic.php?f=10&t=35469. @@ -4517,9 +4511,9 @@ int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool /*isFine*/, bool // Eq. 18.33 // double mu = grad.dot(xdir) / ( (1.-rho) * resA.lpNorm<1>()); // Eq. 18.36 - mu = std::max(mu, - (grad.dot(xdir) + std::max(0., 0.5*xdir.dot(B*xdir))) / - ( (1. - rho) * resA.lpNorm<1>() ) ); + mu = std::max(mu, + (grad.dot(xdir) + std::max(0., 0.5 * xdir.dot(B * xdir))) + / ((1. - rho) * resA.lpNorm<1>())); // Eq. 18.27 double f0 = subsysB->error() + mu * resA.lpNorm<1>(); @@ -4528,71 +4522,71 @@ int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool /*isFine*/, bool double deriv = grad.dot(xdir) - mu * resA.lpNorm<1>(); x = x0 + alpha * xdir; - subsysA->setParams(plistAB,x); - subsysB->setParams(plistAB,x); + subsysA->setParams(plistAB, x); + subsysB->setParams(plistAB, x); subsysA->calcResidual(resA); double f = subsysB->error() + mu * resA.lpNorm<1>(); // line search, Eq. 18.28 bool first = true; while (f > f0 + eta * alpha * deriv) { - if (first) { // try a second order step -// xdir1 = JA.jacobiSvd(Eigen::ComputeThinU | -// Eigen::ComputeThinV).solve(-resA); - xdir1 = -Y*resA; - x += xdir1; // = x0 + alpha * xdir + xdir1 - subsysA->setParams(plistAB,x); - subsysB->setParams(plistAB,x); + if (first) { + // try a second order step + // xdir1 = JA.jacobiSvd(Eigen::ComputeThinU | + // Eigen::ComputeThinV).solve(-resA); + xdir1 = -Y * resA; + x += xdir1;// = x0 + alpha * xdir + xdir1 + subsysA->setParams(plistAB, x); + subsysB->setParams(plistAB, x); subsysA->calcResidual(resA); f = subsysB->error() + mu * resA.lpNorm<1>(); if (f < f0 + eta * alpha * deriv) break; } alpha = tau * alpha; - if (alpha < 1e-8) // let the linesearch fail + if (alpha < 1e-8)// let the linesearch fail alpha = 0.; x = x0 + alpha * xdir; - subsysA->setParams(plistAB,x); - subsysB->setParams(plistAB,x); + subsysA->setParams(plistAB, x); + subsysB->setParams(plistAB, x); subsysA->calcResidual(resA); f = subsysB->error() + mu * resA.lpNorm<1>(); - if (alpha < 1e-8) // let the linesearch fail + if (alpha < 1e-8)// let the linesearch fail break; } lambda = lambda0 + alpha * lambdadir; - } h = x - x0; y = grad - JA.transpose() * lambda; { - subsysB->calcGrad(plistAB,grad); - subsysA->calcJacobi(plistAB,JA); + subsysB->calcGrad(plistAB, grad); + subsysA->calcJacobi(plistAB, JA); subsysA->calcResidual(resA); } - y = grad - JA.transpose() * lambda - y; // Eq. 18.13 + y = grad - JA.transpose() * lambda - y;// Eq. 18.13 if (iter > 1) { double yTh = y.dot(h); if (yTh != 0) { Bh = B * h; - //Now calculate the BFGS update on B - B += 1./yTh * y * y.transpose(); - B -= 1./h.dot(Bh) * (Bh * Bh.transpose()); + // Now calculate the BFGS update on B + B += 1. / yTh * y * y.transpose(); + B -= 1. / h.dot(Bh) * (Bh * Bh.transpose()); } } double err = subsysA->error(); - if (h.norm() <= (isRedundantsolving?convergenceRedundant:convergence) && err <= smallF) + if (h.norm() <= (isRedundantsolving ? convergenceRedundant : convergence) && err <= smallF) break; - if (err > divergingLim || err != err) // check for diverging and NaN + if (err > divergingLim || err != err)// check for diverging and NaN break; } int ret; if (subsysA->error() <= smallF) ret = Success; - else if (h.norm() <= (isRedundantsolving?convergenceRedundant:convergence)) + else if (h.norm() <= (isRedundantsolving ? convergenceRedundant : convergence)) ret = Converged; else ret = Failed; @@ -4600,18 +4594,18 @@ int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool /*isFine*/, bool subsysA->revertParams(); subsysB->revertParams(); return ret; - } void System::applySolution() { - for (int cid=0; cid < int(subSystems.size()); cid++) { + for (int cid = 0; cid < int(subSystems.size()); cid++) { if (subSystemsAux[cid]) subSystemsAux[cid]->applySolution(); if (subSystems[cid]) subSystems[cid]->applySolution(); - for (MAP_pD_pD::const_iterator it=reductionmaps[cid].begin(); - it != reductionmaps[cid].end(); ++it) + for (MAP_pD_pD::const_iterator it = reductionmaps[cid].begin(); + it != reductionmaps[cid].end(); + ++it) *(it->first) = *(it->second); } } @@ -4621,13 +4615,11 @@ void System::undoSolution() resetToReference(); } -void System::makeReducedJacobian(Eigen::MatrixXd &J, - std::map &jacobianconstraintmap, - GCS::VEC_pD &pdiagnoselist, - std::map< int , int> &tagmultiplicity) +void System::makeReducedJacobian(Eigen::MatrixXd& J, std::map& jacobianconstraintmap, + GCS::VEC_pD& pdiagnoselist, std::map& tagmultiplicity) { // construct specific parameter list for diagonose ignoring driven constraint parameters - for (int j=0; j < int(plist.size()); j++) { + for (int j = 0; j < int(plist.size()); j++) { auto result1 = std::find(std::begin(pdrivenlist), std::end(pdrivenlist), plist[j]); if (result1 == std::end(pdrivenlist)) { @@ -4638,29 +4630,30 @@ void System::makeReducedJacobian(Eigen::MatrixXd &J, J = Eigen::MatrixXd::Zero(clist.size(), pdiagnoselist.size()); - int jacobianconstraintcount=0; - int allcount=0; - for (std::vector::iterator constr=clist.begin(); constr != clist.end(); ++constr) { + int jacobianconstraintcount = 0; + int allcount = 0; + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); + ++constr) { (*constr)->revertParams(); ++allcount; if ((*constr)->getTag() >= 0 && (*constr)->isDriving()) { jacobianconstraintcount++; - for (int j=0; j < int(pdiagnoselist.size()); j++) { - J(jacobianconstraintcount-1,j) = (*constr)->grad(pdiagnoselist[j]); + for (int j = 0; j < int(pdiagnoselist.size()); j++) { + J(jacobianconstraintcount - 1, j) = (*constr)->grad(pdiagnoselist[j]); } // parallel processing: create tag multiplicity map - if(tagmultiplicity.find((*constr)->getTag()) == tagmultiplicity.end()) + if (tagmultiplicity.find((*constr)->getTag()) == tagmultiplicity.end()) tagmultiplicity[(*constr)->getTag()] = 0; else tagmultiplicity[(*constr)->getTag()]++; - jacobianconstraintmap[jacobianconstraintcount-1] = allcount-1; + jacobianconstraintmap[jacobianconstraintcount - 1] = allcount - 1; } } - if(jacobianconstraintcount == 0) // only driven constraints - J.resize(0,0); + if (jacobianconstraintcount == 0)// only driven constraints + J.resize(0, 0); } int System::diagnose(Algorithm alg) @@ -4682,7 +4675,7 @@ int System::diagnose(Algorithm alg) } #ifdef _DEBUG_TO_FILE -SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); + SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); #endif // Input parameters' lists: @@ -4718,7 +4711,7 @@ SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); // maps the index of the rows of the reduced jacobian matrix (solver constraints) to // the index those constraints would have in a full size Jacobian matrix - std::map jacobianconstraintmap; + std::map jacobianconstraintmap; // list of parameters to be diagnosed in this routine (removes value parameters from driven // constraints) @@ -4737,63 +4730,69 @@ SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); hasDiagnosis = true; dofs = pdiagnoselist.size(); - if(J.rows() > 0) + if (J.rows() > 0) emptyDiagnoseMatrix = false; - // There is a legacy decision to use QR decomposition. I (abdullah) do not know all the - // consideration taken in that decisions. I see that: - // - QR decomposition is able to provide information about the rank and redundant/conflicting - // constraints - // - The QR decomposition of J and the QR decomposition of the transpose of J are unrelated - // (for reasons see below): - // https://mathoverflow.net/questions/338729/translate-between-qr-decomposition-of-a-and-a-transpose - // - QR is cheaper than a SVD decomposition - // - QR is more expensive than a rank revealing LU factorization - // - QR is less stable than SVD with respect to rank - // - It is unclear whether it is possible to obtain information about redundancy with SVD and LU + // There is a legacy decision to use QR decomposition. I (abdullah) do not know all the + // consideration taken in that decisions. I see that: + // - QR decomposition is able to provide information about the rank and + // redundant/conflicting + // constraints + // - The QR decomposition of J and the QR decomposition of the transpose of J are unrelated + // (for reasons see below): + // https://mathoverflow.net/questions/338729/translate-between-qr-decomposition-of-a-and-a-transpose + // - QR is cheaper than a SVD decomposition + // - QR is more expensive than a rank revealing LU factorization + // - QR is less stable than SVD with respect to rank + // - It is unclear whether it is possible to obtain information about redundancy with SVD + // and LU - // Given this legacy decision, the following is observed: - // - A = QR decomposition can be used for the diagonise of dependency of the "columns" of A. the - // reason is that matrix R is upper triangular with columns of A showing the dependencies. - // - The same does not apply to the "rows". - // - For this reason, to enable a full diagnose of constraints, a QR decomposition must be done - // on the transpose of the Jacobian matrix (J), this is JT. + // Given this legacy decision, the following is observed: + // - A = QR decomposition can be used for the diagonise of dependency of the "columns" of A. + // the reason is that matrix R is upper triangular with columns of A showing the + // dependencies. + // - The same does not apply to the "rows". + // - For this reason, to enable a full diagnose of constraints, a QR decomposition must be + // done + // on the transpose of the Jacobian matrix (J), this is JT. - // Eigen capabilities: - // - If Eigen full pivoting QR decomposition is used, it is possible to track the rows of JT - // during the decomposition. This can be leveraged to identify a set of independent rows of - // JT (geometry) that form a rank N basis. However, because the R matrix is of the JT - // decomposition and not the J decomposition, it is not possible to reduce the system to - // identify exactly which rows are dependent. - // - The effect is that it provides a set of parameters of geometry that are not constraint, - // but it does not identify ALL geometries that are not fixed. - // - If SpareQR is used, then it is not possible to track the rows of JT during decomposition. - // I do not know if it is still possible to obtain geometry information at all from SparseQR. - // After several years these questions remain open: - // https://stackoverflow.com/questions/49009771/getting-rows-transpositions-with-sparse-qr - // https://forum.kde.org/viewtopic.php?f=74&t=151239 - // - // Implementation below: - // - // Two QR decompositions are used below. One for diagnosis of constraints and a second one for - // diagnosis of parameters, i.e. to identify whether the parameter is fully constraint - // (independent) or not (i.e. it is dependent). + // Eigen capabilities: + // - If Eigen full pivoting QR decomposition is used, it is possible to track the rows of JT + // during the decomposition. This can be leveraged to identify a set of independent rows + // of JT (geometry) that form a rank N basis. However, because the R matrix is of the JT + // decomposition and not the J decomposition, it is not possible to reduce the system to + // identify exactly which rows are dependent. + // - The effect is that it provides a set of parameters of geometry that are not constraint, + // but it does not identify ALL geometries that are not fixed. + // - If SpareQR is used, then it is not possible to track the rows of JT during + // decomposition. + // I do not know if it is still possible to obtain geometry information at all from + // SparseQR. After several years these questions remain open: + // https://stackoverflow.com/questions/49009771/getting-rows-transpositions-with-sparse-qr + // https://forum.kde.org/viewtopic.php?f=74&t=151239 + // + // Implementation below: + // + // Two QR decompositions are used below. One for diagnosis of constraints and a second one + // for diagnosis of parameters, i.e. to identify whether the parameter is fully constraint + // (independent) or not (i.e. it is dependent). - // QR decomposition method selection: SparseQR vs DenseQR + // QR decomposition method selection: SparseQR vs DenseQR #ifndef EIGEN_SPARSEQR_COMPATIBLE - if(qrAlgorithm==EigenSparseQR){ - Base::Console().Warning("SparseQR not supported by you current version of Eigen. It requires Eigen 3.2.2 or higher. Falling back to Dense QR\n"); - qrAlgorithm=EigenDenseQR; + if (qrAlgorithm == EigenSparseQR) { + Base::Console().Warning("SparseQR not supported by you current version of Eigen. It " + "requires Eigen 3.2.2 or higher. Falling back to Dense QR\n"); + qrAlgorithm = EigenDenseQR; } #endif - if(qrAlgorithm==EigenDenseQR){ - #ifdef PROFILE_DIAGNOSE + if (qrAlgorithm == EigenDenseQR) { +#ifdef PROFILE_DIAGNOSE Base::TimeInfo DenseQR_start_time; - #endif +#endif if (J.rows() > 0) { - int rank = 0; // rank is not cheap to retrieve from qrJT in DenseQR + int rank = 0;// rank is not cheap to retrieve from qrJT in DenseQR Eigen::MatrixXd R; Eigen::FullPivHouseholderQR qrJT; // Here we give the system the possibility to run the two QR decompositions in parallel, @@ -4808,9 +4807,14 @@ SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); // // identifyDependentParametersDenseQR(J, jacobianconstraintmap, pdiagnoselist, true) // - auto fut = std::async(&System::identifyDependentParametersDenseQR,this,J,jacobianconstraintmap, pdiagnoselist, true); + auto fut = std::async(&System::identifyDependentParametersDenseQR, + this, + J, + jacobianconstraintmap, + pdiagnoselist, + true); - makeDenseQRDecomposition( J, jacobianconstraintmap, qrJT, rank, R); + makeDenseQRDecomposition(J, jacobianconstraintmap, qrJT, rank, R); int paramsNum = qrJT.rows(); int constrNum = qrJT.cols(); @@ -4821,9 +4825,9 @@ SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); // identifyDependentGeometryParametersInTransposedJacobianDenseQRDecomposition( qrJT, // pdiagnoselist, paramsNum, rank); - fut.wait(); // wait for the execution of identifyDependentParametersSparseQR to finish + fut.wait();// wait for the execution of identifyDependentParametersSparseQR to finish - dofs = paramsNum - rank; // unless overconstraint, which will be overridden below + dofs = paramsNum - rank;// unless overconstraint, which will be overridden below // Detecting conflicting or redundant constraints if (constrNum > rank) {// conflicting or redundant constraints @@ -4841,24 +4845,24 @@ SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); dofs = paramsNum - nonredundantconstrNum; } } - #ifdef PROFILE_DIAGNOSE +#ifdef PROFILE_DIAGNOSE Base::TimeInfo DenseQR_end_time; - auto SolveTime = Base::TimeInfo::diffTimeF(DenseQR_start_time,DenseQR_end_time); + auto SolveTime = Base::TimeInfo::diffTimeF(DenseQR_start_time, DenseQR_end_time); Base::Console().Log("\nDenseQR - Lapsed Time: %f seconds\n", SolveTime); - #endif +#endif } #ifdef EIGEN_SPARSEQR_COMPATIBLE - else if(qrAlgorithm==EigenSparseQR){ - #ifdef PROFILE_DIAGNOSE + else if (qrAlgorithm == EigenSparseQR) { +#ifdef PROFILE_DIAGNOSE Base::TimeInfo SparseQR_start_time; - #endif +#endif if (J.rows() > 0) { int rank = 0; Eigen::MatrixXd R; - Eigen::SparseQR, Eigen::COLAMDOrdering > SqrJT; + Eigen::SparseQR, Eigen::COLAMDOrdering> SqrJT; // Here we give the system the possibility to run the two QR decompositions in parallel, // depending on the load of the system so we are using the default std::launch::async | // std::launch::deferred policy, as nobody better than the system nows if it can run the @@ -4873,8 +4877,8 @@ SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); // // Debug: // auto fut = - // std::async(std::launch::deferred,&System::identifyDependentParametersSparseQR,this,J,jacobianconstraintmap, - // pdiagnoselist, false); + // std::async(std::launch::deferred,&System::identifyDependentParametersSparseQR, this, + // J, jacobianconstraintmap, pdiagnoselist, false); auto fut = std::async(&System::identifyDependentParametersSparseQR, this, J, @@ -4893,7 +4897,7 @@ SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); dofs = paramsNum - rank;// unless overconstraint, which will be overridden below // Detecting conflicting or redundant constraints - if (constrNum > rank) {// conflicting or redundant constraints + if (constrNum > rank) { int nonredundantconstrNum; @@ -4912,28 +4916,28 @@ SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n"); } } - #ifdef PROFILE_DIAGNOSE +#ifdef PROFILE_DIAGNOSE Base::TimeInfo SparseQR_end_time; - auto SolveTime = Base::TimeInfo::diffTimeF(SparseQR_start_time,SparseQR_end_time); + auto SolveTime = Base::TimeInfo::diffTimeF(SparseQR_start_time, SparseQR_end_time); Base::Console().Log("\nSparseQR - Lapsed Time: %f seconds\n", SolveTime); - #endif +#endif } #endif return dofs; } -void System::makeDenseQRDecomposition( const Eigen::MatrixXd &J, - const std::map &jacobianconstraintmap, - Eigen::FullPivHouseholderQR& qrJT, - int &rank, Eigen::MatrixXd & R, bool transposeJ, bool silent) +void System::makeDenseQRDecomposition(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + Eigen::FullPivHouseholderQR& qrJT, int& rank, + Eigen::MatrixXd& R, bool transposeJ, bool silent) { #ifdef _GCS_DEBUG - if(!silent) - SolverReportingManager::Manager().LogMatrix("J",J); + if (!silent) + SolverReportingManager::Manager().LogMatrix("J", J); #endif #ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX @@ -4949,7 +4953,7 @@ void System::makeDenseQRDecomposition( const Eigen::MatrixXd &J, if (J.rows() > 0) { Eigen::MatrixXd JG; - if(transposeJ) + if (transposeJ) JG = J.topRows(jacobianconstraintmap.size()).transpose(); else JG = J.topRows(jacobianconstraintmap.size()); @@ -4966,8 +4970,7 @@ void System::makeDenseQRDecomposition( const Eigen::MatrixXd &J, if (colsNum >= rowsNum) R = qrJT.matrixQR().triangularView(); else - R = qrJT.matrixQR().topRows(colsNum) - .triangularView(); + R = qrJT.matrixQR().topRows(colsNum).triangularView(); } else { rowsNum = JG.rows(); @@ -4980,7 +4983,7 @@ void System::makeDenseQRDecomposition( const Eigen::MatrixXd &J, #endif } - if(debugMode==IterationLevel && !silent) { + if (debugMode == IterationLevel && !silent) { SolverReportingManager::Manager().LogQRSystemInformation(*this, rowsNum, colsNum, rank); } @@ -5082,28 +5085,26 @@ void System::makeSparseQRDecomposition( } #endif// EIGEN_SPARSEQR_COMPATIBLE -void System::identifyDependentParametersDenseQR( const Eigen::MatrixXd &J, - const std::map &jacobianconstraintmap, - const GCS::VEC_pD &pdiagnoselist, - bool silent) +void System::identifyDependentParametersDenseQR(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + const GCS::VEC_pD& pdiagnoselist, bool silent) { Eigen::FullPivHouseholderQR qrJ; Eigen::MatrixXd Rparams; int rank; - makeDenseQRDecomposition( J, jacobianconstraintmap, qrJ, rank, Rparams, false, true); + makeDenseQRDecomposition(J, jacobianconstraintmap, qrJ, rank, Rparams, false, true); identifyDependentParameters(qrJ, Rparams, rank, pdiagnoselist, silent); } #ifdef EIGEN_SPARSEQR_COMPATIBLE -void System::identifyDependentParametersSparseQR( const Eigen::MatrixXd &J, - const std::map &jacobianconstraintmap, - const GCS::VEC_pD &pdiagnoselist, - bool silent) +void System::identifyDependentParametersSparseQR(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + const GCS::VEC_pD& pdiagnoselist, bool silent) { - Eigen::SparseQR, Eigen::COLAMDOrdering > SqrJ; + Eigen::SparseQR, Eigen::COLAMDOrdering> SqrJ; Eigen::MatrixXd Rparams; int nontransprank; @@ -5120,39 +5121,36 @@ void System::identifyDependentParametersSparseQR( const Eigen::MatrixXd &J, } #endif -template -void System::identifyDependentParameters( T & qrJ, - Eigen::MatrixXd &Rparams, - int rank, - const GCS::VEC_pD &pdiagnoselist, - bool silent) +template +void System::identifyDependentParameters(T& qrJ, Eigen::MatrixXd& Rparams, int rank, + const GCS::VEC_pD& pdiagnoselist, bool silent) { (void)silent;// silent is only used in debug code, but it is important as Base::Console is not // thread-safe. Removes warning in non Debug mode. - //int constrNum = SqrJ.rows(); // this is the other way around than for the transposed J - //int paramsNum = SqrJ.cols(); + // int constrNum = SqrJ.rows(); // this is the other way around than for the transposed J + // int paramsNum = SqrJ.cols(); eliminateNonZerosOverPivotInUpperTriangularMatrix(Rparams, rank); #ifdef _GCS_DEBUG - if(!silent) + if (!silent) SolverReportingManager::Manager().LogMatrix("Rparams_nonzeros_over_pilot", Rparams); #endif - pDependentParametersGroups.resize(qrJ.cols()-rank); - for (int j=rank; j < qrJ.cols(); j++) { - for (int row=0; row < rank; row++) { - if (fabs(Rparams(row,j)) > 1e-10) { + pDependentParametersGroups.resize(qrJ.cols() - rank); + for (int j = rank; j < qrJ.cols(); j++) { + for (int row = 0; row < rank; row++) { + if (fabs(Rparams(row, j)) > 1e-10) { int origCol = qrJ.colsPermutation().indices()[row]; - pDependentParametersGroups[j-rank].push_back(pdiagnoselist[origCol]); + pDependentParametersGroups[j - rank].push_back(pdiagnoselist[origCol]); pDependentParameters.push_back(pdiagnoselist[origCol]); } } int origCol = qrJ.colsPermutation().indices()[j]; - pDependentParametersGroups[j-rank].push_back(pdiagnoselist[origCol]); + pDependentParametersGroups[j - rank].push_back(pdiagnoselist[origCol]); pDependentParameters.push_back(pdiagnoselist[origCol]); } @@ -5169,9 +5167,8 @@ void System::identifyDependentParameters( T & qrJ, } void System::identifyDependentGeometryParametersInTransposedJacobianDenseQRDecomposition( - const Eigen::FullPivHouseholderQR& qrJT, - const GCS::VEC_pD &pdiagnoselist, - int paramsNum, int rank) + const Eigen::FullPivHouseholderQR& qrJT, const GCS::VEC_pD& pdiagnoselist, + int paramsNum, int rank) { // DETECTING CONSTRAINT SOLVER PARAMETERS // @@ -5192,7 +5189,7 @@ void System::identifyDependentGeometryParametersInTransposedJacobianDenseQRDecom // P.J.P' = Q.R see https://eigen.tuxfamily.org/dox/classEigen_1_1FullPivHouseholderQR.html const MatrixIndexType rowTranspositions = qrJT.rowsTranspositions(); - for(int k = 0; k < rank; ++k) + for (int k = 0; k < rank; ++k) rowPermutations.applyTranspositionOnTheRight(k, rowTranspositions.coeff(k)); #ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX @@ -5223,9 +5220,9 @@ void System::identifyDependentGeometryParametersInTransposedJacobianDenseQRDecom #endif // If not independent, must be dependent - for(int j=0; j < paramsNum; j++) { + for (int j = 0; j < paramsNum; j++) { auto result = indepParamCols.find(j); - if(result == indepParamCols.end()) { + if (result == indepParamCols.end()) { depParamCols.insert(j); } } @@ -5234,12 +5231,12 @@ void System::identifyDependentGeometryParametersInTransposedJacobianDenseQRDecom stream.flush(); stream << "Indep params: ["; - for(auto indep :indepParamCols) + for (auto indep : indepParamCols) stream << indep; stream << "]" << std::endl; stream << "Dep params: ["; - for(auto dep :depParamCols) + for (auto dep : depParamCols) stream << dep; stream << "]" << std::endl; @@ -5248,28 +5245,28 @@ void System::identifyDependentGeometryParametersInTransposedJacobianDenseQRDecom #endif - for( auto param : depParamCols) { + for (auto param : depParamCols) { pDependentParameters.push_back(pdiagnoselist[param]); } - } -void System::eliminateNonZerosOverPivotInUpperTriangularMatrix( Eigen::MatrixXd &R, int rank) +void System::eliminateNonZerosOverPivotInUpperTriangularMatrix(Eigen::MatrixXd& R, int rank) { - for (int i=1; i < rank; i++) { + for (int i = 1; i < rank; i++) { // eliminate non zeros above pivot - assert(R(i,i) != 0); - for (int row=0; row < i; row++) { - if (R(row,i) != 0) { - double coef=R(row,i)/R(i,i); - R.block(row,i+1,1,R.cols()-i-1) -= coef * R.block(i,i+1,1,R.cols()-i-1); - R(row,i) = 0; + assert(R(i, i) != 0); + for (int row = 0; row < i; row++) { + if (R(row, i) != 0) { + double coef = R(row, i) / R(i, i); + R.block(row, i + 1, 1, R.cols() - i - 1) -= + coef * R.block(i, i + 1, 1, R.cols() - i - 1); + R(row, i) = 0; } } } } -template +template void System::identifyConflictingRedundantConstraints( Algorithm alg, const T& qrJT, const std::map& jacobianconstraintmap, const std::map& tagmultiplicity, GCS::VEC_pD& pdiagnoselist, Eigen::MatrixXd& R, @@ -5277,18 +5274,18 @@ void System::identifyConflictingRedundantConstraints( { eliminateNonZerosOverPivotInUpperTriangularMatrix(R, rank); - std::vector< std::vector > conflictGroups(constrNum-rank); - for (int j=rank; j < constrNum; j++) { - for (int row=0; row < rank; row++) { - if (fabs(R(row,j)) > 1e-10) { + std::vector> conflictGroups(constrNum - rank); + for (int j = rank; j < constrNum; j++) { + for (int row = 0; row < rank; row++) { + if (fabs(R(row, j)) > 1e-10) { int origCol = qrJT.colsPermutation().indices()[row]; - conflictGroups[j-rank].push_back(clist[jacobianconstraintmap.at(origCol)]); + conflictGroups[j - rank].push_back(clist[jacobianconstraintmap.at(origCol)]); } } int origCol = qrJT.colsPermutation().indices()[j]; - conflictGroups[j-rank].push_back(clist[jacobianconstraintmap.at(origCol)]); + conflictGroups[j - rank].push_back(clist[jacobianconstraintmap.at(origCol)]); } // Augment the information regarding the group of constraints that are conflicting or redundant. @@ -5300,7 +5297,7 @@ void System::identifyConflictingRedundantConstraints( // try to remove the conflicting constraints and solve the // system in order to check if the removed constraints were // just redundant but not really conflicting - std::set skipped; + std::set skipped; SET_I satisfiedGroups; while (1) { // conflictingMap contains all the eligible constraints of conflict groups not yet @@ -5376,12 +5373,12 @@ void System::identifyConflictingRedundantConstraints( // constraint associated with the same sketcher constraint that is also conflicting) auto maxPopularityTag = mostPopular->getTag(); - for(const auto & c : conflictingMap) { - if(c.first->getTag() == maxPopularityTag) { + for (const auto& c : conflictingMap) { + if (c.first->getTag() == maxPopularityTag) { skipped.insert(c.first); for (SET_I::const_iterator it = conflictingMap[c.first].begin(); - it != conflictingMap[c.first].end(); - ++it) { + it != conflictingMap[c.first].end(); + ++it) { satisfiedGroups.insert(*it); } } @@ -5390,42 +5387,43 @@ void System::identifyConflictingRedundantConstraints( } // Augment information regarding the choice made by popularity contest - if(debugMode==IterationLevel) { + if (debugMode == IterationLevel) { SolverReportingManager::Manager().LogSetOfConstraints("Chosen redundants", skipped); } - std::vector clistTmp; + std::vector clistTmp; clistTmp.reserve(clist.size()); - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) { + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); + ++constr) { if ((*constr)->isDriving() && skipped.count(*constr) == 0) clistTmp.push_back(*constr); } - SubSystem *subSysTmp = new SubSystem(clistTmp, pdiagnoselist); - int res = solve(subSysTmp,true,alg,true); + SubSystem* subSysTmp = new SubSystem(clistTmp, pdiagnoselist); + int res = solve(subSysTmp, true, alg, true); - if(debugMode==Minimal || debugMode==IterationLevel) { + if (debugMode == Minimal || debugMode == IterationLevel) { std::string solvername; switch (alg) { case 0: solvername = "BFGS"; break; - case 1: // solving with the LevenbergMarquardt solver + case 1:// solving with the LevenbergMarquardt solver solvername = "LevenbergMarquardt"; break; - case 2: // solving with the BFGS solver + case 2:// solving with the BFGS solver solvername = "DogLeg"; break; } - Base::Console().Log("Sketcher::RedundantSolving-%s-\n",solvername.c_str()); + Base::Console().Log("Sketcher::RedundantSolving-%s-\n", solvername.c_str()); } if (res == Success) { subSysTmp->applySolution(); - for (std::set::const_iterator constr=skipped.begin(); - constr != skipped.end(); ++constr) { + for (std::set::const_iterator constr = skipped.begin(); + constr != skipped.end(); + ++constr) { double err = (*constr)->error(); if (err * err < convergenceRedundant) redundant.insert(*constr); @@ -5476,8 +5474,7 @@ void System::identifyConflictingRedundantConstraints( } conflictingTags.resize(conflictingTagsSet.size()); - std::copy(conflictingTagsSet.begin(), conflictingTagsSet.end(), - conflictingTags.begin()); + std::copy(conflictingTagsSet.begin(), conflictingTagsSet.end(), conflictingTags.begin()); // output of redundant tags SET_I redundantTagsSet, partiallyRedundantTagsSet; @@ -5488,20 +5485,20 @@ void System::identifyConflictingRedundantConstraints( } // remove tags represented at least in one non-redundant constraint - for (std::vector::iterator constr=clist.begin(); constr != clist.end(); ++constr) + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); ++constr) if (redundant.count(*constr) == 0) redundantTagsSet.erase((*constr)->getTag()); redundantTags.resize(redundantTagsSet.size()); - std::copy(redundantTagsSet.begin(), redundantTagsSet.end(), - redundantTags.begin()); + std::copy(redundantTagsSet.begin(), redundantTagsSet.end(), redundantTags.begin()); - for(auto r : redundantTagsSet) + for (auto r : redundantTagsSet) partiallyRedundantTagsSet.erase(r); partiallyRedundantTags.resize(partiallyRedundantTagsSet.size()); - std::copy(partiallyRedundantTagsSet.begin(), partiallyRedundantTagsSet.end(), - partiallyRedundantTags.begin()); + std::copy(partiallyRedundantTagsSet.begin(), + partiallyRedundantTagsSet.end(), + partiallyRedundantTags.begin()); nonredundantconstrNum = constrNum; } @@ -5516,39 +5513,39 @@ void System::clearSubSystems() subSystemsAux.clear(); } -double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir) +double lineSearch(SubSystem* subsys, Eigen::VectorXd& xdir) { - double f1,f2,f3,alpha1,alpha2,alpha3,alphaStar; + double f1, f2, f3, alpha1, alpha2, alpha3, alphaStar; double alphaMax = subsys->maxStep(xdir); Eigen::VectorXd x0, x; - //Save initial values + // Save initial values subsys->getParams(x0); - //Start at the initial position alpha1 = 0 + // Start at the initial position alpha1 = 0 alpha1 = 0.; f1 = subsys->error(); - //Take a step of alpha2 = 1 + // Take a step of alpha2 = 1 alpha2 = 1.; x = x0 + alpha2 * xdir; subsys->setParams(x); f2 = subsys->error(); - //Take a step of alpha3 = 2*alpha2 - alpha3 = alpha2*2; + // Take a step of alpha3 = 2*alpha2 + alpha3 = alpha2 * 2; x = x0 + alpha3 * xdir; subsys->setParams(x); f3 = subsys->error(); - //Now reduce or lengthen alpha2 and alpha3 until the minimum is - //Bracketed by the triplet f1>f2f2 f1 || f2 > f3) { if (f2 > f1) { - //If f2 is greater than f1 then we shorten alpha2 and alpha3 closer to f1 - //Effectively both are shortened by a factor of two. + // If f2 is greater than f1 then we shorten alpha2 and alpha3 closer to f1 + // Effectively both are shortened by a factor of two. alpha3 = alpha2; f3 = f2; alpha2 = alpha2 / 2; @@ -5559,8 +5556,8 @@ double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir) else if (f2 > f3) { if (alpha3 >= alphaMax) break; - //If f2 is greater than f3 then we increase alpha2 and alpha3 away from f1 - //Effectively both are lengthened by a factor of two. + // If f2 is greater than f3 then we increase alpha2 and alpha3 away from f1 + // Effectively both are lengthened by a factor of two. alpha2 = alpha3; f2 = f3; alpha3 = alpha3 * 2; @@ -5569,10 +5566,10 @@ double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir) f3 = subsys->error(); } } - //Get the alpha for the minimum f of the quadratic approximation - alphaStar = alpha2 + ((alpha2-alpha1)*(f1-f3))/(3*(f1-2*f2+f3)); + // Get the alpha for the minimum f of the quadratic approximation + alphaStar = alpha2 + ((alpha2 - alpha1) * (f1 - f3)) / (3 * (f1 - 2 * f2 + f3)); - //Guarantee that the new alphaStar is within the bracket + // Guarantee that the new alphaStar is within the bracket if (alphaStar >= alpha3 || alphaStar <= alpha1) alphaStar = alpha2; @@ -5582,7 +5579,7 @@ double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir) if (alphaStar != alphaStar) alphaStar = 0.; - //Take a final step to alphaStar + // Take a final step to alphaStar x = x0 + alphaStar * xdir; subsys->setParams(x); @@ -5590,50 +5587,50 @@ double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir) } -void free(VEC_pD &doublevec) +void free(VEC_pD& doublevec) { - for (VEC_pD::iterator it = doublevec.begin(); - it != doublevec.end(); ++it) { - if (*it) delete *it; + for (VEC_pD::iterator it = doublevec.begin(); it != doublevec.end(); ++it) { + if (*it) + delete *it; } doublevec.clear(); } -void free(std::vector &constrvec) +void free(std::vector& constrvec) { - for (std::vector::iterator constr=constrvec.begin(); - constr != constrvec.end(); ++constr) { + for (std::vector::iterator constr = constrvec.begin(); constr != constrvec.end(); + ++constr) { if (*constr) { switch ((*constr)->getTypeId()) { case Equal: - delete static_cast(*constr); + delete static_cast(*constr); break; case Difference: - delete static_cast(*constr); + delete static_cast(*constr); break; case P2PDistance: - delete static_cast(*constr); + delete static_cast(*constr); break; case P2PAngle: - delete static_cast(*constr); + delete static_cast(*constr); break; case P2LDistance: - delete static_cast(*constr); + delete static_cast(*constr); break; case PointOnLine: - delete static_cast(*constr); + delete static_cast(*constr); break; case Parallel: - delete static_cast(*constr); + delete static_cast(*constr); break; case Perpendicular: - delete static_cast(*constr); + delete static_cast(*constr); break; case L2LAngle: - delete static_cast(*constr); + delete static_cast(*constr); break; case MidpointOnLine: - delete static_cast(*constr); + delete static_cast(*constr); break; case None: default: @@ -5644,13 +5641,13 @@ void free(std::vector &constrvec) constrvec.clear(); } -void free(std::vector &subsysvec) +void free(std::vector& subsysvec) { - for (std::vector::iterator it=subsysvec.begin(); - it != subsysvec.end(); ++it) { - if (*it) delete *it; + for (std::vector::iterator it = subsysvec.begin(); it != subsysvec.end(); ++it) { + if (*it) + delete *it; } } -} //namespace GCS +}// namespace GCS diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index a4cf60b230..b8a379bfd9 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -25,32 +25,31 @@ #include -#include "SubSystem.h" #include "../../SketcherGlobal.h" +#include "SubSystem.h" -#define EIGEN_VERSION (EIGEN_WORLD_VERSION * 10000 \ -+ EIGEN_MAJOR_VERSION * 100 \ -+ EIGEN_MINOR_VERSION) +#define EIGEN_VERSION \ + (EIGEN_WORLD_VERSION * 10000 + EIGEN_MAJOR_VERSION * 100 + EIGEN_MINOR_VERSION) #if EIGEN_VERSION >= 30202 - #define EIGEN_SPARSEQR_COMPATIBLE - #include +#define EIGEN_SPARSEQR_COMPATIBLE +#include #endif namespace GCS { - /////////////////////////////////////// - // Other BFGS Solver parameters - /////////////////////////////////////// - #define XconvergenceRough 1e-8 - #define smallF 1e-20 +/////////////////////////////////////// +// Other BFGS Solver parameters +/////////////////////////////////////// +#define XconvergenceRough 1e-8 +#define smallF 1e-20 - /////////////////////////////////////// - // Solver - /////////////////////////////////////// +/////////////////////////////////////// +// Solver +/////////////////////////////////////// - enum SolveStatus +enum SolveStatus { Success = 0, // Found a solution zeroing the error function Converged = 1, // Found a solution minimizing the error function @@ -59,422 +58,418 @@ namespace GCS // resulting geometry is OCE-invalid }; - enum Algorithm { - BFGS = 0, - LevenbergMarquardt = 1, - DogLeg = 2 - }; +enum Algorithm +{ + BFGS = 0, + LevenbergMarquardt = 1, + DogLeg = 2 +}; - enum DogLegGaussStep { - FullPivLU = 0, - LeastNormFullPivLU = 1, - LeastNormLdlt = 2 - }; +enum DogLegGaussStep +{ + FullPivLU = 0, + LeastNormFullPivLU = 1, + LeastNormLdlt = 2 +}; - enum QRAlgorithm { - EigenDenseQR = 0, - EigenSparseQR = 1 - }; +enum QRAlgorithm +{ + EigenDenseQR = 0, + EigenSparseQR = 1 +}; - enum DebugMode { - NoDebug = 0, - Minimal = 1, - IterationLevel = 2 - }; +enum DebugMode +{ + NoDebug = 0, + Minimal = 1, + IterationLevel = 2 +}; - // Magic numbers for Constraint tags - // - Positive Tags identify a higher level constraint form which the solver constraint - // originates - // - Negative Tags represent temporary constraints, used for example in moving operations, these - // have a different handling in component splitting, see GCS::initSolution. Lifetime is defined - // by the container object via GCS::clearByTag. - // - -1 is typically used as tag for these temporary constraints, its parameters are - // enforced with - // a lower priority than the main system (real sketcher constraints). It gives a nice - // effect when dragging the edge of an unconstrained circle, that the center won't move - // if the edge can be dragged, and only when/if the edge cannot be dragged, e.g. radius - // constraint, the center is moved). - enum SpecialTag - { - DefaultTemporaryConstraint = -1 - }; +// Magic numbers for Constraint tags +// - Positive Tags identify a higher level constraint form which the solver constraint +// originates +// - Negative Tags represent temporary constraints, used for example in moving operations, these +// have a different handling in component splitting, see GCS::initSolution. Lifetime is defined +// by the container object via GCS::clearByTag. +// - -1 is typically used as tag for these temporary constraints, its parameters are +// enforced with +// a lower priority than the main system (real sketcher constraints). It gives a nice +// effect when dragging the edge of an unconstrained circle, that the center won't move +// if the edge can be dragged, and only when/if the edge cannot be dragged, e.g. radius +// constraint, the center is moved). +enum SpecialTag +{ + DefaultTemporaryConstraint = -1 +}; - class SketcherExport System - { +class SketcherExport System +{ // This is the main class. It holds all constraints and information // about partitioning into subsystems and solution strategies - private: - VEC_pD plist; // list of the unknown parameters - VEC_pD pdrivenlist; // list of parameters of driven constraints - MAP_pD_I pIndex; +private: + VEC_pD plist; // list of the unknown parameters + VEC_pD pdrivenlist;// list of parameters of driven constraints + MAP_pD_I pIndex; - VEC_pD pDependentParameters; // list of dependent parameters by the system + VEC_pD pDependentParameters;// list of dependent parameters by the system - // This is a map of primary and secondary identifiers that are found dependent by the solver - // GCS ignores from a type point - std::vector< std::vector > pDependentParametersGroups; + // This is a map of primary and secondary identifiers that are found dependent by the solver + // GCS ignores from a type point + std::vector> pDependentParametersGroups; - std::vector clist; - std::map c2p; // constraint to parameter adjacency list - std::map > p2c; // parameter to constraint adjacency list + std::vector clist; + std::map c2p; // constraint to parameter adjacency list + std::map> p2c;// parameter to constraint adjacency list - std::vector subSystems, subSystemsAux; - void clearSubSystems(); + std::vector subSystems, subSystemsAux; + void clearSubSystems(); - VEC_D reference; - void setReference(); // copies the current parameter values to reference - void resetToReference(); // reverts all parameter values to the stored reference + VEC_D reference; + void setReference(); // copies the current parameter values to reference + void resetToReference();// reverts all parameter values to the stored reference - std::vector plists;// partitioned plist except equality constraints - std::vector> - clists; // partitioned clist except equality constraints - std::vector reductionmaps;// for simplification of equality constraints + std::vector plists;// partitioned plist except equality constraints + // partitioned clist except equality constraints + std::vector> clists; + std::vector reductionmaps;// for simplification of equality constraints - int dofs; - std::set redundant; - VEC_I conflictingTags, redundantTags, partiallyRedundantTags; + int dofs; + std::set redundant; + VEC_I conflictingTags, redundantTags, partiallyRedundantTags; - bool hasUnknowns; // if plist is filled with the unknown parameters - bool hasDiagnosis; // if dofs, conflictingTags, redundantTags are up to date - bool isInit; // if plists, clists, reductionmaps are up to date + bool hasUnknowns; // if plist is filled with the unknown parameters + bool hasDiagnosis;// if dofs, conflictingTags, redundantTags are up to date + bool isInit; // if plists, clists, reductionmaps are up to date - bool emptyDiagnoseMatrix; // false only if there is at least one driving constraint. + bool emptyDiagnoseMatrix;// false only if there is at least one driving constraint. - int solve_BFGS(SubSystem *subsys, bool isFine=true, bool isRedundantsolving=false); - int solve_LM(SubSystem *subsys, bool isRedundantsolving=false); - int solve_DL(SubSystem *subsys, bool isRedundantsolving=false); + int solve_BFGS(SubSystem* subsys, bool isFine = true, bool isRedundantsolving = false); + int solve_LM(SubSystem* subsys, bool isRedundantsolving = false); + int solve_DL(SubSystem* subsys, bool isRedundantsolving = false); - void makeReducedJacobian(Eigen::MatrixXd& J, std::map& jacobianconstraintmap, - GCS::VEC_pD& pdiagnoselist, std::map& tagmultiplicity); + void makeReducedJacobian(Eigen::MatrixXd& J, std::map& jacobianconstraintmap, + GCS::VEC_pD& pdiagnoselist, std::map& tagmultiplicity); - void makeDenseQRDecomposition(const Eigen::MatrixXd& J, - const std::map& jacobianconstraintmap, - Eigen::FullPivHouseholderQR& qrJT, int& rank, - Eigen::MatrixXd& R, bool transposeJ = true, - bool silent = false); + void makeDenseQRDecomposition(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + Eigen::FullPivHouseholderQR& qrJT, int& rank, + Eigen::MatrixXd& R, bool transposeJ = true, bool silent = false); #ifdef EIGEN_SPARSEQR_COMPATIBLE - void makeSparseQRDecomposition( - const Eigen::MatrixXd& J, const std::map& jacobianconstraintmap, - Eigen::SparseQR, Eigen::COLAMDOrdering>& SqrJT, - int& rank, Eigen::MatrixXd& R, bool transposeJ = true, bool silent = false); + void makeSparseQRDecomposition( + const Eigen::MatrixXd& J, const std::map& jacobianconstraintmap, + Eigen::SparseQR, Eigen::COLAMDOrdering>& SqrJT, int& rank, + Eigen::MatrixXd& R, bool transposeJ = true, bool silent = false); #endif - // This function name is long for a reason: - // - Only for DenseQR - // - Only for Transposed Jacobian QR decomposition - void identifyDependentGeometryParametersInTransposedJacobianDenseQRDecomposition( - const Eigen::FullPivHouseholderQR& qrJT, - const GCS::VEC_pD& pdiagnoselist, int paramsNum, int rank); + // This function name is long for a reason: + // - Only for DenseQR + // - Only for Transposed Jacobian QR decomposition + void identifyDependentGeometryParametersInTransposedJacobianDenseQRDecomposition( + const Eigen::FullPivHouseholderQR& qrJT, const GCS::VEC_pD& pdiagnoselist, + int paramsNum, int rank); - template - void identifyConflictingRedundantConstraints( - Algorithm alg, const T& qrJT, const std::map& jacobianconstraintmap, - const std::map& tagmultiplicity, GCS::VEC_pD& pdiagnoselist, - Eigen::MatrixXd& R, int constrNum, int rank, int& nonredundantconstrNum); - - void eliminateNonZerosOverPivotInUpperTriangularMatrix(Eigen::MatrixXd& R, int rank); - -#ifdef EIGEN_SPARSEQR_COMPATIBLE - void identifyDependentParametersSparseQR(const Eigen::MatrixXd& J, + template + void identifyConflictingRedundantConstraints(Algorithm alg, const T& qrJT, const std::map& jacobianconstraintmap, - const GCS::VEC_pD& pdiagnoselist, - bool silent = true); + const std::map& tagmultiplicity, + GCS::VEC_pD& pdiagnoselist, Eigen::MatrixXd& R, + int constrNum, int rank, + int& nonredundantconstrNum); + + void eliminateNonZerosOverPivotInUpperTriangularMatrix(Eigen::MatrixXd& R, int rank); + +#ifdef EIGEN_SPARSEQR_COMPATIBLE + void identifyDependentParametersSparseQR(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + const GCS::VEC_pD& pdiagnoselist, bool silent = true); #endif - void identifyDependentParametersDenseQR(const Eigen::MatrixXd& J, - const std::map& jacobianconstraintmap, - const GCS::VEC_pD& pdiagnoselist, - bool silent = true); + void identifyDependentParametersDenseQR(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + const GCS::VEC_pD& pdiagnoselist, bool silent = true); - template - void identifyDependentParameters(T& qrJ, Eigen::MatrixXd& Rparams, int rank, - const GCS::VEC_pD& pdiagnoselist, bool silent = true); + template + void identifyDependentParameters(T& qrJ, Eigen::MatrixXd& Rparams, int rank, + const GCS::VEC_pD& pdiagnoselist, bool silent = true); - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ - void extractSubsystem(SubSystem *subsys, bool isRedundantsolving); - #endif - public: - int maxIter; - int maxIterRedundant; - bool sketchSizeMultiplier;// if true note that the total number of iterations allowed is - // MaxIterations *xLength - bool sketchSizeMultiplierRedundant; - double convergence; - double convergenceRedundant; - QRAlgorithm qrAlgorithm; - DogLegGaussStep dogLegGaussStep; - double qrpivotThreshold; - DebugMode debugMode; - double LM_eps; - double LM_eps1; - double LM_tau; - double DL_tolg; - double DL_tolx; - double DL_tolf; - double LM_epsRedundant; - double LM_eps1Redundant; - double LM_tauRedundant; - double DL_tolgRedundant; - double DL_tolxRedundant; - double DL_tolfRedundant; +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + void extractSubsystem(SubSystem* subsys, bool isRedundantsolving); +#endif +public: + int maxIter; + int maxIterRedundant; + bool sketchSizeMultiplier;// if true note that the total number of iterations allowed is + // MaxIterations *xLength + bool sketchSizeMultiplierRedundant; + double convergence; + double convergenceRedundant; + QRAlgorithm qrAlgorithm; + DogLegGaussStep dogLegGaussStep; + double qrpivotThreshold; + DebugMode debugMode; + double LM_eps; + double LM_eps1; + double LM_tau; + double DL_tolg; + double DL_tolx; + double DL_tolf; + double LM_epsRedundant; + double LM_eps1Redundant; + double LM_tauRedundant; + double DL_tolgRedundant; + double DL_tolxRedundant; + double DL_tolfRedundant; - public: - System(); - /*System(std::vector clist_);*/ - ~System(); +public: + System(); + /*System(std::vector clist_);*/ + ~System(); - void clear(); - void clearByTag(int tagId); + void clear(); + void clearByTag(int tagId); - int addConstraint(Constraint *constr); - void removeConstraint(Constraint *constr); + int addConstraint(Constraint* constr); + void removeConstraint(Constraint* constr); - // basic constraints - int addConstraintEqual( - double* param1, double* param2, int tagId = 0, bool driving = true, - Constraint::Alignment internalalignment = Constraint::Alignment::NoInternalAlignment); - int addConstraintProportional(double* param1, double* param2, double ratio, int tagId, - bool driving = true); - int addConstraintDifference(double* param1, double* param2, double* difference, - int tagId = 0, bool driving = true); - int addConstraintP2PDistance(Point& p1, Point& p2, double* distance, int tagId = 0, - bool driving = true); - int addConstraintP2PAngle(Point& p1, Point& p2, double* angle, double incrAngle, - int tagId = 0, bool driving = true); - int addConstraintP2PAngle(Point& p1, Point& p2, double* angle, int tagId = 0, + // basic constraints + int addConstraintEqual( + double* param1, double* param2, int tagId = 0, bool driving = true, + Constraint::Alignment internalalignment = Constraint::Alignment::NoInternalAlignment); + int addConstraintProportional(double* param1, double* param2, double ratio, int tagId, bool driving = true); - int addConstraintP2LDistance(Point& p, Line& l, double* distance, int tagId = 0, - bool driving = true); - int addConstraintPointOnLine(Point& p, Line& l, int tagId = 0, bool driving = true); - int addConstraintPointOnLine(Point& p, Point& lp1, Point& lp2, int tagId = 0, - bool driving = true); - int addConstraintPointOnPerpBisector(Point& p, Line& l, int tagId = 0, bool driving = true); - int addConstraintPointOnPerpBisector(Point& p, Point& lp1, Point& lp2, int tagId = 0, - bool driving = true); - int addConstraintParallel(Line& l1, Line& l2, int tagId = 0, bool driving = true); - int addConstraintPerpendicular(Line& l1, Line& l2, int tagId = 0, bool driving = true); - int addConstraintPerpendicular(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, - int tagId = 0, bool driving = true); - int addConstraintL2LAngle(Line& l1, Line& l2, double* angle, int tagId = 0, - bool driving = true); - int addConstraintL2LAngle(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, double* angle, - int tagId = 0, bool driving = true); - int addConstraintAngleViaPoint(Curve& crv1, Curve& crv2, Point& p, double* angle, - int tagId = 0, bool driving = true); - int addConstraintMidpointOnLine(Line& l1, Line& l2, int tagId = 0, bool driving = true); - int addConstraintMidpointOnLine(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, - int tagId = 0, bool driving = true); - int addConstraintTangentCircumf(Point& p1, Point& p2, double* rd1, double* rd2, - bool internal = false, int tagId = 0, bool driving = true); - int addConstraintTangentAtBSplineKnot(BSpline& b, Line& l, unsigned int knotindex, - int tagId = 0, bool driving = true); - - // derived constraints - int addConstraintP2PCoincident(Point& p1, Point& p2, int tagId = 0, bool driving = true); - int addConstraintHorizontal(Line& l, int tagId = 0, bool driving = true); - int addConstraintHorizontal(Point& p1, Point& p2, int tagId = 0, bool driving = true); - int addConstraintVertical(Line& l, int tagId = 0, bool driving = true); - int addConstraintVertical(Point& p1, Point& p2, int tagId = 0, bool driving = true); - int addConstraintCoordinateX(Point& p, double* x, int tagId = 0, bool driving = true); - int addConstraintCoordinateY(Point& p, double* y, int tagId = 0, bool driving = true); - int addConstraintArcRules(Arc& a, int tagId = 0, bool driving = true); - int addConstraintPointOnCircle(Point& p, Circle& c, int tagId = 0, bool driving = true); - int addConstraintPointOnEllipse(Point& p, Ellipse& e, int tagId = 0, bool driving = true); - int addConstraintPointOnHyperbolicArc(Point& p, ArcOfHyperbola& e, int tagId = 0, - bool driving = true); - int addConstraintPointOnParabolicArc(Point& p, ArcOfParabola& e, int tagId = 0, - bool driving = true); - int addConstraintPointOnBSpline(Point& p, BSpline& b, double* pointparam, int tagId, - bool driving = true); - int addConstraintArcOfEllipseRules(ArcOfEllipse& a, int tagId = 0, bool driving = true); - int addConstraintCurveValue(Point& p, Curve& a, double* u, int tagId = 0, - bool driving = true); - int addConstraintArcOfHyperbolaRules(ArcOfHyperbola& a, int tagId = 0, bool driving = true); - int addConstraintArcOfParabolaRules(ArcOfParabola& a, int tagId = 0, bool driving = true); - int addConstraintPointOnArc(Point& p, Arc& a, int tagId = 0, bool driving = true); - int addConstraintPerpendicularLine2Arc(Point& p1, Point& p2, Arc& a, int tagId = 0, - bool driving = true); - int addConstraintPerpendicularArc2Line(Arc& a, Point& p1, Point& p2, int tagId = 0, - bool driving = true); - int addConstraintPerpendicularCircle2Arc(Point& center, double* radius, Arc& a, - int tagId = 0, bool driving = true); - int addConstraintPerpendicularArc2Circle(Arc& a, Point& center, double* radius, - int tagId = 0, bool driving = true); - int addConstraintPerpendicularArc2Arc(Arc& a1, bool reverse1, Arc& a2, bool reverse2, - int tagId = 0, bool driving = true); - int addConstraintTangent(Line& l, Circle& c, int tagId = 0, bool driving = true); - int addConstraintTangent(Line& l, Ellipse& e, int tagId = 0, bool driving = true); - int addConstraintTangent(Line& l, Arc& a, int tagId = 0, bool driving = true); - int addConstraintTangent(Circle& c1, Circle& c2, int tagId = 0, bool driving = true); - int addConstraintTangent(Arc& a1, Arc& a2, int tagId = 0, bool driving = true); - int addConstraintTangent(Circle& c, Arc& a, int tagId = 0, bool driving = true); - - int addConstraintCircleRadius(Circle& c, double* radius, int tagId = 0, - bool driving = true); - int addConstraintArcRadius(Arc& a, double* radius, int tagId = 0, bool driving = true); - int addConstraintCircleDiameter(Circle& c, double* diameter, int tagId = 0, - bool driving = true); - int addConstraintArcDiameter(Arc& a, double* diameter, int tagId = 0, bool driving = true); - int addConstraintEqualLength(Line& l1, Line& l2, int tagId = 0, bool driving = true); - int addConstraintEqualRadius(Circle& c1, Circle& c2, int tagId = 0, bool driving = true); - int addConstraintEqualRadii(Ellipse& e1, Ellipse& e2, int tagId = 0, bool driving = true); - int addConstraintEqualRadii(ArcOfHyperbola& a1, ArcOfHyperbola& a2, int tagId = 0, - bool driving = true); - int addConstraintEqualRadius(Circle& c1, Arc& a2, int tagId = 0, bool driving = true); - int addConstraintEqualRadius(Arc& a1, Arc& a2, int tagId = 0, bool driving = true); - int addConstraintEqualFocus(ArcOfParabola& a1, ArcOfParabola& a2, int tagId = 0, - bool driving = true); - int addConstraintP2PSymmetric(Point& p1, Point& p2, Line& l, int tagId = 0, - bool driving = true); - int addConstraintP2PSymmetric(Point& p1, Point& p2, Point& p, int tagId = 0, - bool driving = true); - int addConstraintSnellsLaw(Curve& ray1, Curve& ray2, Curve& boundary, Point p, double* n1, - double* n2, bool flipn1, bool flipn2, int tagId, + int addConstraintDifference(double* param1, double* param2, double* difference, int tagId = 0, + bool driving = true); + int addConstraintP2PDistance(Point& p1, Point& p2, double* distance, int tagId = 0, + bool driving = true); + int addConstraintP2PAngle(Point& p1, Point& p2, double* angle, double incrAngle, int tagId = 0, + bool driving = true); + int addConstraintP2PAngle(Point& p1, Point& p2, double* angle, int tagId = 0, + bool driving = true); + int addConstraintP2LDistance(Point& p, Line& l, double* distance, int tagId = 0, + bool driving = true); + int addConstraintPointOnLine(Point& p, Line& l, int tagId = 0, bool driving = true); + int addConstraintPointOnLine(Point& p, Point& lp1, Point& lp2, int tagId = 0, + bool driving = true); + int addConstraintPointOnPerpBisector(Point& p, Line& l, int tagId = 0, bool driving = true); + int addConstraintPointOnPerpBisector(Point& p, Point& lp1, Point& lp2, int tagId = 0, + bool driving = true); + int addConstraintParallel(Line& l1, Line& l2, int tagId = 0, bool driving = true); + int addConstraintPerpendicular(Line& l1, Line& l2, int tagId = 0, bool driving = true); + int addConstraintPerpendicular(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, + int tagId = 0, bool driving = true); + int addConstraintL2LAngle(Line& l1, Line& l2, double* angle, int tagId = 0, + bool driving = true); + int addConstraintL2LAngle(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, double* angle, + int tagId = 0, bool driving = true); + int addConstraintAngleViaPoint(Curve& crv1, Curve& crv2, Point& p, double* angle, int tagId = 0, bool driving = true); + int addConstraintMidpointOnLine(Line& l1, Line& l2, int tagId = 0, bool driving = true); + int addConstraintMidpointOnLine(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, + int tagId = 0, bool driving = true); + int addConstraintTangentCircumf(Point& p1, Point& p2, double* rd1, double* rd2, + bool internal = false, int tagId = 0, bool driving = true); + int addConstraintTangentAtBSplineKnot(BSpline& b, Line& l, unsigned int knotindex, + int tagId = 0, bool driving = true); - 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); + // derived constraints + int addConstraintP2PCoincident(Point& p1, Point& p2, int tagId = 0, bool driving = true); + int addConstraintHorizontal(Line& l, int tagId = 0, bool driving = true); + int addConstraintHorizontal(Point& p1, Point& p2, int tagId = 0, bool driving = true); + int addConstraintVertical(Line& l, int tagId = 0, bool driving = true); + int addConstraintVertical(Point& p1, Point& p2, int tagId = 0, bool driving = true); + int addConstraintCoordinateX(Point& p, double* x, int tagId = 0, bool driving = true); + int addConstraintCoordinateY(Point& p, double* y, int tagId = 0, bool driving = true); + int addConstraintArcRules(Arc& a, int tagId = 0, bool driving = true); + int addConstraintPointOnCircle(Point& p, Circle& c, int tagId = 0, bool driving = true); + int addConstraintPointOnEllipse(Point& p, Ellipse& e, int tagId = 0, bool driving = true); + int addConstraintPointOnHyperbolicArc(Point& p, ArcOfHyperbola& e, int tagId = 0, + bool driving = true); + int addConstraintPointOnParabolicArc(Point& p, ArcOfParabola& e, int tagId = 0, + bool driving = true); + int addConstraintPointOnBSpline(Point& p, BSpline& b, double* pointparam, int tagId, + bool driving = true); + int addConstraintArcOfEllipseRules(ArcOfEllipse& a, int tagId = 0, bool driving = true); + int addConstraintCurveValue(Point& p, Curve& a, double* u, int tagId = 0, bool driving = true); + int addConstraintArcOfHyperbolaRules(ArcOfHyperbola& a, int tagId = 0, bool driving = true); + int addConstraintArcOfParabolaRules(ArcOfParabola& a, int tagId = 0, bool driving = true); + int addConstraintPointOnArc(Point& p, Arc& a, int tagId = 0, bool driving = true); + int addConstraintPerpendicularLine2Arc(Point& p1, Point& p2, Arc& a, int tagId = 0, + bool driving = true); + int addConstraintPerpendicularArc2Line(Arc& a, Point& p1, Point& p2, int tagId = 0, + bool driving = true); + int addConstraintPerpendicularCircle2Arc(Point& center, double* radius, Arc& a, int tagId = 0, + bool driving = true); + int addConstraintPerpendicularArc2Circle(Arc& a, Point& center, double* radius, int tagId = 0, + bool driving = true); + int addConstraintPerpendicularArc2Arc(Arc& a1, bool reverse1, Arc& a2, bool reverse2, + int tagId = 0, bool driving = true); + int addConstraintTangent(Line& l, Circle& c, int tagId = 0, bool driving = true); + int addConstraintTangent(Line& l, Ellipse& e, int tagId = 0, bool driving = true); + int addConstraintTangent(Line& l, Arc& a, int tagId = 0, bool driving = true); + int addConstraintTangent(Circle& c1, Circle& c2, int tagId = 0, bool driving = true); + int addConstraintTangent(Arc& a1, Arc& a2, int tagId = 0, bool driving = true); + int addConstraintTangent(Circle& c, Arc& a, int tagId = 0, bool driving = true); - // internal alignment constraints - int addConstraintInternalAlignmentPoint2Ellipse(Ellipse& e, Point& p1, - InternalAlignmentType alignmentType, - int tagId = 0, bool driving = true); - int addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse& e, Point& p1, Point& p2, - int tagId = 0, bool driving = true); - int addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse& e, Point& p1, Point& p2, - int tagId = 0, bool driving = true); - int addConstraintInternalAlignmentEllipseFocus1(Ellipse& e, Point& p1, int tagId = 0, - bool driving = true); - int addConstraintInternalAlignmentEllipseFocus2(Ellipse& e, Point& p1, int tagId = 0, - bool driving = true); - int addConstraintInternalAlignmentPoint2Hyperbola(Hyperbola& e, Point& p1, - InternalAlignmentType alignmentType, - int tagId = 0, bool driving = true); - int addConstraintInternalAlignmentHyperbolaMajorDiameter(Hyperbola& e, Point& p1, Point& p2, - int tagId = 0, - bool driving = true); - int addConstraintInternalAlignmentHyperbolaMinorDiameter(Hyperbola& e, Point& p1, Point& p2, - int tagId = 0, - bool driving = true); - int addConstraintInternalAlignmentHyperbolaFocus(Hyperbola& e, Point& p1, int tagId = 0, - bool driving = true); - int addConstraintInternalAlignmentParabolaFocus(Parabola& e, Point& p1, int tagId = 0, - bool driving = true); - int addConstraintInternalAlignmentBSplineControlPoint(BSpline& b, Circle& c, - unsigned int poleindex, int tag = 0, - bool driving = true); - int addConstraintInternalAlignmentKnotPoint(BSpline& b, Point& p, unsigned int knotindex, + int addConstraintCircleRadius(Circle& c, double* radius, int tagId = 0, bool driving = true); + int addConstraintArcRadius(Arc& a, double* radius, int tagId = 0, bool driving = true); + int addConstraintCircleDiameter(Circle& c, double* diameter, int tagId = 0, + bool driving = true); + int addConstraintArcDiameter(Arc& a, double* diameter, int tagId = 0, bool driving = true); + int addConstraintEqualLength(Line& l1, Line& l2, int tagId = 0, bool driving = true); + int addConstraintEqualRadius(Circle& c1, Circle& c2, int tagId = 0, bool driving = true); + int addConstraintEqualRadii(Ellipse& e1, Ellipse& e2, int tagId = 0, bool driving = true); + int addConstraintEqualRadii(ArcOfHyperbola& a1, ArcOfHyperbola& a2, int tagId = 0, + bool driving = true); + int addConstraintEqualRadius(Circle& c1, Arc& a2, int tagId = 0, bool driving = true); + int addConstraintEqualRadius(Arc& a1, Arc& a2, int tagId = 0, bool driving = true); + int addConstraintEqualFocus(ArcOfParabola& a1, ArcOfParabola& a2, int tagId = 0, + bool driving = true); + int addConstraintP2PSymmetric(Point& p1, Point& p2, Line& l, int tagId = 0, + bool driving = true); + int addConstraintP2PSymmetric(Point& p1, Point& p2, Point& p, int tagId = 0, + bool driving = true); + int addConstraintSnellsLaw(Curve& ray1, Curve& ray2, Curve& boundary, Point p, double* n1, + double* n2, bool flipn1, bool flipn2, int tagId, + bool driving = true); + + 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, + InternalAlignmentType alignmentType, int tagId = 0, bool driving = true); + int addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse& e, Point& p1, Point& p2, + int tagId = 0, bool driving = true); + int addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse& e, Point& p1, Point& p2, + int tagId = 0, bool driving = true); + int addConstraintInternalAlignmentEllipseFocus1(Ellipse& e, Point& p1, int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentEllipseFocus2(Ellipse& e, Point& p1, int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentPoint2Hyperbola(Hyperbola& e, Point& p1, + InternalAlignmentType alignmentType, + int tagId = 0, bool driving = true); + int addConstraintInternalAlignmentHyperbolaMajorDiameter(Hyperbola& e, Point& p1, Point& p2, + int tagId = 0, bool driving = true); + int addConstraintInternalAlignmentHyperbolaMinorDiameter(Hyperbola& e, Point& p1, Point& p2, + int tagId = 0, bool driving = true); + int addConstraintInternalAlignmentHyperbolaFocus(Hyperbola& e, Point& p1, int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentParabolaFocus(Parabola& e, Point& p1, int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentBSplineControlPoint(BSpline& b, Circle& c, + unsigned int poleindex, int tag = 0, + bool driving = true); + int addConstraintInternalAlignmentKnotPoint(BSpline& b, Point& p, unsigned int knotindex, + int tagId = 0, bool driving = true); - double calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p) const; - double calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p1, - Point& p2) const; - void calculateNormalAtPoint(const Curve& crv, const Point& p, double& rtnX, - double& rtnY) const; + double calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p) const; + double calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p1, Point& p2) const; + void calculateNormalAtPoint(const Curve& crv, const Point& p, double& rtnX, double& rtnY) const; - // Calculates errors of all constraints which have a tag equal to - // the one supplied. Individual errors are summed up using RMS. - // If none are found, NAN is returned - // If there's only one, a signed value is returned. - // Effectively, it calculates the error of a UI constraint - double calculateConstraintErrorByTag(int tagId); + // Calculates errors of all constraints which have a tag equal to + // the one supplied. Individual errors are summed up using RMS. + // If none are found, NAN is returned + // If there's only one, a signed value is returned. + // Effectively, it calculates the error of a UI constraint + double calculateConstraintErrorByTag(int tagId); - void rescaleConstraint(int id, double coeff); + void rescaleConstraint(int id, double coeff); - void declareUnknowns(VEC_pD ¶ms); - void declareDrivenParams(VEC_pD ¶ms); - void initSolution(Algorithm alg=DogLeg); + void declareUnknowns(VEC_pD& params); + void declareDrivenParams(VEC_pD& params); + void initSolution(Algorithm alg = DogLeg); - int solve(bool isFine = true, Algorithm alg = DogLeg, bool isRedundantsolving = false); - int solve(VEC_pD& params, bool isFine = true, Algorithm alg = DogLeg, - bool isRedundantsolving = false); - int solve(SubSystem* subsys, bool isFine = true, Algorithm alg = DogLeg, - bool isRedundantsolving = false); - int solve(SubSystem* subsysA, SubSystem* subsysB, bool isFine = true, - bool isRedundantsolving = false); + int solve(bool isFine = true, Algorithm alg = DogLeg, bool isRedundantsolving = false); + int solve(VEC_pD& params, bool isFine = true, Algorithm alg = DogLeg, + bool isRedundantsolving = false); + int solve(SubSystem* subsys, bool isFine = true, Algorithm alg = DogLeg, + bool isRedundantsolving = false); + int solve(SubSystem* subsysA, SubSystem* subsysB, bool isFine = true, + bool isRedundantsolving = false); - void applySolution(); - void undoSolution(); - // FIXME: looks like XconvergenceFine is not the solver precision, at least in DogLeg - // solver. - // Note: Yes, every solver has a different way of interpreting precision - // but one has to study what is this needed for in order to decide - // what to return (this is unchanged from previous versions) - double getFinePrecision() - { - return convergence; + void applySolution(); + void undoSolution(); + // FIXME: looks like XconvergenceFine is not the solver precision, at least in DogLeg + // solver. + // Note: Yes, every solver has a different way of interpreting precision + // but one has to study what is this needed for in order to decide + // what to return (this is unchanged from previous versions) + double getFinePrecision() + { + return convergence; + } + + int diagnose(Algorithm alg = DogLeg); + int dofsNumber() const + { + return hasDiagnosis ? dofs : -1; + } + void getConflicting(VEC_I& conflictingOut) const + { + conflictingOut = hasDiagnosis ? conflictingTags : VEC_I(0); + } + void getRedundant(VEC_I& redundantOut) const + { + redundantOut = hasDiagnosis ? redundantTags : VEC_I(0); + } + void getPartiallyRedundant(VEC_I& partiallyredundantOut) const + { + partiallyredundantOut = hasDiagnosis ? partiallyRedundantTags : VEC_I(0); + } + void getDependentParams(VEC_pD& pdependentparameterlist) const + { + pdependentparameterlist = pDependentParameters; + } + void + getDependentParamsGroups(std::vector>& pdependentparametergroups) const + { + pdependentparametergroups = pDependentParametersGroups; + } + bool isEmptyDiagnoseMatrix() const + { + return emptyDiagnoseMatrix; + } + + bool hasConflicting() const + { + return !(hasDiagnosis && conflictingTags.empty()); + } + bool hasRedundant() const + { + return !(hasDiagnosis && redundantTags.empty()); + } + bool hasPartiallyRedundant() const + { + return !(hasDiagnosis && partiallyRedundantTags.empty()); + } + + void invalidatedDiagnosis(); + + // Unit testing interface - not intended for use by production code +protected: + size_t _getNumberOfConstraints(int tagID = -1) + { + if (tagID < 0) { + return clist.size(); } - - int diagnose(Algorithm alg = DogLeg); - int dofsNumber() const - { - return hasDiagnosis ? dofs : -1; - } - void getConflicting(VEC_I& conflictingOut) const - { - conflictingOut = hasDiagnosis ? conflictingTags : VEC_I(0); - } - void getRedundant(VEC_I& redundantOut) const - { - redundantOut = hasDiagnosis ? redundantTags : VEC_I(0); - } - void getPartiallyRedundant(VEC_I& partiallyredundantOut) const - { - partiallyredundantOut = hasDiagnosis ? partiallyRedundantTags : VEC_I(0); - } - void getDependentParams(VEC_pD& pdependentparameterlist) const - { - pdependentparameterlist = pDependentParameters; - } - void - getDependentParamsGroups(std::vector>& pdependentparametergroups) const - { - pdependentparametergroups = pDependentParametersGroups; - } - bool isEmptyDiagnoseMatrix() const - { - return emptyDiagnoseMatrix; - } - - bool hasConflicting() const - { - return !(hasDiagnosis && conflictingTags.empty()); - } - bool hasRedundant() const - { - return !(hasDiagnosis && redundantTags.empty()); - } - bool hasPartiallyRedundant() const - { - return !(hasDiagnosis && partiallyRedundantTags.empty()); - } - - void invalidatedDiagnosis(); - - // Unit testing interface - not intended for use by production code - protected: - size_t _getNumberOfConstraints(int tagID = -1) - { - if (tagID < 0) { - return clist.size(); - } - return std::count_if(clist.begin(), clist.end(), [tagID](Constraint* constraint) { - return constraint->getTag() == tagID; - }); - } - }; + return std::count_if(clist.begin(), clist.end(), [tagID](Constraint* constraint) { + return constraint->getTag() == tagID; + }); + } +}; - /////////////////////////////////////// - // Helper elements - /////////////////////////////////////// +/////////////////////////////////////// +// Helper elements +/////////////////////////////////////// - void free(VEC_pD &doublevec); - void free(std::vector &constrvec); - void free(std::vector &subsysvec); +void free(VEC_pD& doublevec); +void free(std::vector& constrvec); +void free(std::vector& subsysvec); -} //namespace GCS +}// namespace GCS -#endif // PLANEGCS_GCS_H +#endif// PLANEGCS_GCS_H diff --git a/src/Mod/Sketcher/App/planegcs/Geo.cpp b/src/Mod/Sketcher/App/planegcs/Geo.cpp index ff2ddb1d7b..a10d65c242 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.cpp +++ b/src/Mod/Sketcher/App/planegcs/Geo.cpp @@ -29,9 +29,10 @@ #include "Geo.h" -namespace GCS{ +namespace GCS +{ -DeriVector2::DeriVector2(const Point &p, const double *derivparam) +DeriVector2::DeriVector2(const Point& p, const double* derivparam) { x = *p.x; y = *p.y; @@ -43,7 +44,7 @@ DeriVector2::DeriVector2(const Point &p, const double *derivparam) dy = 1.0; } -double DeriVector2::length(double &dlength) const +double DeriVector2::length(double& dlength) const { double l = length(); if (l == 0) { @@ -77,7 +78,7 @@ DeriVector2 DeriVector2::getNormalized() const } } -double DeriVector2::scalarProd(const DeriVector2 &v2, double *dprd) const +double DeriVector2::scalarProd(const DeriVector2& v2, double* dprd) const { if (dprd) { *dprd = dx * v2.x + x * v2.dx + dy * v2.y + y * v2.dy; @@ -91,10 +92,10 @@ 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 +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; + 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 @@ -105,9 +106,9 @@ DeriVector2 Curve::Value(double /*u*/, double /*du*/, const double* /*derivparam //----------------Line -DeriVector2 Line::CalculateNormal(const Point &p, const double* derivparam) const +DeriVector2 Line::CalculateNormal(const Point& p, const double* derivparam) const { - (void) p; + (void)p; DeriVector2 p1v(p1, derivparam); DeriVector2 p2v(p2, derivparam); @@ -120,24 +121,32 @@ DeriVector2 Line::Value(double u, double du, const double* derivparam) const DeriVector2 p2v(p2, derivparam); DeriVector2 line_vec = p2v.subtr(p1v); - return p1v.sum(line_vec.multD(u,du)); + return p1v.sum(line_vec.multD(u, du)); } -int Line::PushOwnParams(VEC_pD &pvec) +int Line::PushOwnParams(VEC_pD& pvec) { - int cnt=0; - pvec.push_back(p1.x); cnt++; - pvec.push_back(p1.y); cnt++; - pvec.push_back(p2.x); cnt++; - pvec.push_back(p2.y); cnt++; + int cnt = 0; + pvec.push_back(p1.x); + cnt++; + pvec.push_back(p1.y); + cnt++; + pvec.push_back(p2.x); + cnt++; + pvec.push_back(p2.y); + cnt++; return cnt; } -void Line::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void Line::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - p1.x=pvec[cnt]; cnt++; - p1.y=pvec[cnt]; cnt++; - p2.x=pvec[cnt]; cnt++; - p2.y=pvec[cnt]; cnt++; + p1.x = pvec[cnt]; + cnt++; + p1.y = pvec[cnt]; + cnt++; + p2.x = pvec[cnt]; + cnt++; + p2.y = pvec[cnt]; + cnt++; } Line* Line::Copy() { @@ -148,10 +157,10 @@ Line* Line::Copy() //---------------circle -DeriVector2 Circle::CalculateNormal(const Point &p, const double* derivparam) const +DeriVector2 Circle::CalculateNormal(const Point& p, const double* derivparam) const { - DeriVector2 cv (center, derivparam); - DeriVector2 pv (p, derivparam); + DeriVector2 cv(center, derivparam); + DeriVector2 pv(p, derivparam); return cv.subtr(pv); } @@ -172,19 +181,25 @@ DeriVector2 Circle::Value(double u, double du, const double* derivparam) const return cv.sum(ex.multD(co, dco).sum(ey.multD(si, dsi))); } -int Circle::PushOwnParams(VEC_pD &pvec) +int Circle::PushOwnParams(VEC_pD& pvec) { - int cnt=0; - pvec.push_back(center.x); cnt++; - pvec.push_back(center.y); cnt++; - pvec.push_back(rad); cnt++; + int cnt = 0; + pvec.push_back(center.x); + cnt++; + pvec.push_back(center.y); + cnt++; + pvec.push_back(rad); + cnt++; return cnt; } -void Circle::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void Circle::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - center.x=pvec[cnt]; cnt++; - center.y=pvec[cnt]; cnt++; - rad=pvec[cnt]; cnt++; + center.x = pvec[cnt]; + cnt++; + center.y = pvec[cnt]; + cnt++; + rad = pvec[cnt]; + cnt++; } Circle* Circle::Copy() { @@ -193,27 +208,39 @@ Circle* Circle::Copy() } //------------arc -int Arc::PushOwnParams(VEC_pD &pvec) +int Arc::PushOwnParams(VEC_pD& pvec) { - int cnt=0; + int cnt = 0; cnt += Circle::PushOwnParams(pvec); - pvec.push_back(start.x); cnt++; - pvec.push_back(start.y); cnt++; - pvec.push_back(end.x); cnt++; - pvec.push_back(end.y); cnt++; - pvec.push_back(startAngle); cnt++; - pvec.push_back(endAngle); cnt++; + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; + pvec.push_back(startAngle); + cnt++; + pvec.push_back(endAngle); + cnt++; return cnt; } -void Arc::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void Arc::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - Circle::ReconstructOnNewPvec(pvec,cnt); - start.x=pvec[cnt]; cnt++; - start.y=pvec[cnt]; cnt++; - end.x=pvec[cnt]; cnt++; - end.y=pvec[cnt]; cnt++; - startAngle=pvec[cnt]; cnt++; - endAngle=pvec[cnt]; cnt++; + Circle::ReconstructOnNewPvec(pvec, cnt); + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; + startAngle = pvec[cnt]; + cnt++; + endAngle = pvec[cnt]; + cnt++; } Arc* Arc::Copy() { @@ -239,40 +266,40 @@ double Ellipse::getRadMaj(const DeriVector2& center, const DeriVector2& f1, doub return hack.length(ret_dRadMaj); } -//returns major radius. The derivative by derivparam is returned into ret_dRadMaj argument. -double Ellipse::getRadMaj(double *derivparam, double &ret_dRadMaj) const +// returns major radius. The derivative by derivparam is returned into ret_dRadMaj argument. +double Ellipse::getRadMaj(double* derivparam, double& ret_dRadMaj) const { DeriVector2 c(center, derivparam); DeriVector2 f1(focus1, derivparam); - return getRadMaj(c, f1, *radmin, radmin==derivparam ? 1.0 : 0.0, ret_dRadMaj); + return getRadMaj(c, f1, *radmin, radmin == derivparam ? 1.0 : 0.0, ret_dRadMaj); } -//returns the major radius (plain value, no derivatives) +// returns the major radius (plain value, no derivatives) double Ellipse::getRadMaj() const { - double dradmaj;//dummy - return getRadMaj(nullptr,dradmaj); + double dradmaj;// dummy + return getRadMaj(nullptr, dradmaj); } -DeriVector2 Ellipse::CalculateNormal(const Point &p, const double* derivparam) const +DeriVector2 Ellipse::CalculateNormal(const Point& p, const double* derivparam) const { - //fill some vectors in - DeriVector2 cv (center, derivparam); - DeriVector2 f1v (focus1, derivparam); - DeriVector2 pv (p, derivparam); + // fill some vectors in + DeriVector2 cv(center, derivparam); + DeriVector2 f1v(focus1, derivparam); + DeriVector2 pv(p, derivparam); - //calculation. - //focus2: - DeriVector2 f2v = cv.linCombi(2.0, f1v, -1.0); // 2*cv - f1v + // calculation. + // focus2: + DeriVector2 f2v = cv.linCombi(2.0, f1v, -1.0);// 2*cv - f1v - //pf1, pf2 = vectors from p to focus1,focus2 + // pf1, pf2 = vectors from p to focus1,focus2 DeriVector2 pf1 = f1v.subtr(pv); DeriVector2 pf2 = f2v.subtr(pv); - //return sum of normalized pf2, pf2 + // return sum of normalized pf2, pf2 DeriVector2 ret = pf1.getNormalized().sum(pf2.getNormalized()); - //numeric derivatives for testing - #if 0 //make sure to enable DEBUG_DERIVS when enabling +// numeric derivatives for testing +#if 0// make sure to enable DEBUG_DERIVS when enabling if(derivparam) { double const eps = 0.00001; double oldparam = *derivparam; @@ -290,18 +317,18 @@ DeriVector2 Ellipse::CalculateNormal(const Point &p, const double* derivparam) c assert(ret.dy <= std::max(numretl.y,numretr.y) ); assert(ret.dy >= std::min(numretl.y,numretr.y) ); } - #endif +#endif - return ret; + return ret; } DeriVector2 Ellipse::Value(double u, double du, const double* derivparam) const { - //In local coordinate system, value() of ellipse is: + // In local coordinate system, value() of ellipse is: //(a*cos(u), b*sin(u)) - //In global, it is (vector formula): - //center + a_vec*cos(u) + b_vec*sin(u). - //That's what is being computed here. + // In global, it is (vector formula): + // center + a_vec*cos(u) + b_vec*sin(u). + // That's what is being computed here. // DeriVector2 c(this->center, derivparam); @@ -310,41 +337,53 @@ DeriVector2 Ellipse::Value(double u, double du, const double* derivparam) const DeriVector2 emaj = f1.subtr(c).getNormalized(); DeriVector2 emin = emaj.rotate90ccw(); double b, db; - b = *(this->radmin); db = this->radmin==derivparam ? 1.0 : 0.0; + b = *(this->radmin); + db = this->radmin == derivparam ? 1.0 : 0.0; double a, da; - a = this->getRadMaj(c,f1,b,db,da); - DeriVector2 a_vec = emaj.multD(a,da); - DeriVector2 b_vec = emin.multD(b,db); + a = this->getRadMaj(c, f1, b, db, da); + DeriVector2 a_vec = emaj.multD(a, da); + DeriVector2 b_vec = emin.multD(b, db); // // sin, cos with derivatives: double co, dco, si, dsi; - co = std::cos(u); dco = -std::sin(u)*du; - si = std::sin(u); dsi = std::cos(u)*du; + co = std::cos(u); + dco = -std::sin(u) * du; + si = std::sin(u); + dsi = std::cos(u) * du; - DeriVector2 ret; //point of ellipse at parameter value of u, in global coordinates - ret = a_vec.multD(co,dco).sum(b_vec.multD(si,dsi)).sum(c); + DeriVector2 ret;// point of ellipse at parameter value of u, in global coordinates + ret = a_vec.multD(co, dco).sum(b_vec.multD(si, dsi)).sum(c); return ret; - } -int Ellipse::PushOwnParams(VEC_pD &pvec) +int Ellipse::PushOwnParams(VEC_pD& pvec) { - int cnt=0; - pvec.push_back(center.x); cnt++; - pvec.push_back(center.y); cnt++; - pvec.push_back(focus1.x); cnt++; - pvec.push_back(focus1.y); cnt++; - pvec.push_back(radmin); cnt++; + int cnt = 0; + pvec.push_back(center.x); + cnt++; + pvec.push_back(center.y); + cnt++; + pvec.push_back(focus1.x); + cnt++; + pvec.push_back(focus1.y); + cnt++; + pvec.push_back(radmin); + cnt++; return cnt; } -void Ellipse::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void Ellipse::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - center.x=pvec[cnt]; cnt++; - center.y=pvec[cnt]; cnt++; - focus1.x=pvec[cnt]; cnt++; - focus1.y=pvec[cnt]; cnt++; - radmin=pvec[cnt]; cnt++; + center.x = pvec[cnt]; + cnt++; + center.y = pvec[cnt]; + cnt++; + focus1.x = pvec[cnt]; + cnt++; + focus1.y = pvec[cnt]; + cnt++; + radmin = pvec[cnt]; + cnt++; } Ellipse* Ellipse::Copy() { @@ -354,28 +393,39 @@ Ellipse* Ellipse::Copy() //---------------arc of ellipse -int ArcOfEllipse::PushOwnParams(VEC_pD &pvec) +int ArcOfEllipse::PushOwnParams(VEC_pD& pvec) { - int cnt=0; + int cnt = 0; cnt += Ellipse::PushOwnParams(pvec); - pvec.push_back(start.x); cnt++; - pvec.push_back(start.y); cnt++; - pvec.push_back(end.x); cnt++; - pvec.push_back(end.y); cnt++; - pvec.push_back(startAngle); cnt++; - pvec.push_back(endAngle); cnt++; + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; + pvec.push_back(startAngle); + cnt++; + pvec.push_back(endAngle); + cnt++; return cnt; - } -void ArcOfEllipse::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void ArcOfEllipse::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - Ellipse::ReconstructOnNewPvec(pvec,cnt); - start.x=pvec[cnt]; cnt++; - start.y=pvec[cnt]; cnt++; - end.x=pvec[cnt]; cnt++; - end.y=pvec[cnt]; cnt++; - startAngle=pvec[cnt]; cnt++; - endAngle=pvec[cnt]; cnt++; + Ellipse::ReconstructOnNewPvec(pvec, cnt); + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; + startAngle = pvec[cnt]; + cnt++; + endAngle = pvec[cnt]; + cnt++; } ArcOfEllipse* ArcOfEllipse::Copy() { @@ -398,19 +448,19 @@ double Hyperbola::getRadMaj(const DeriVector2& center, const DeriVector2& f1, do return a; } -//returns major radius. The derivative by derivparam is returned into ret_dRadMaj argument. -double Hyperbola::getRadMaj(double *derivparam, double &ret_dRadMaj) const +// returns major radius. The derivative by derivparam is returned into ret_dRadMaj argument. +double Hyperbola::getRadMaj(double* derivparam, double& ret_dRadMaj) const { DeriVector2 c(center, derivparam); DeriVector2 f1(focus1, derivparam); - return getRadMaj(c, f1, *radmin, radmin==derivparam ? 1.0 : 0.0, ret_dRadMaj); + return getRadMaj(c, f1, *radmin, radmin == derivparam ? 1.0 : 0.0, ret_dRadMaj); } -//returns the major radius (plain value, no derivatives) +// returns the major radius (plain value, no derivatives) double Hyperbola::getRadMaj() const { - double dradmaj;//dummy - return getRadMaj(nullptr,dradmaj); + double dradmaj;// dummy + return getRadMaj(nullptr, dradmaj); } DeriVector2 Hyperbola::CalculateNormal(const Point& p, const double* derivparam) const @@ -437,11 +487,11 @@ DeriVector2 Hyperbola::CalculateNormal(const Point& p, const double* derivparam) DeriVector2 Hyperbola::Value(double u, double du, const double* derivparam) const { - //In local coordinate system, value() of hyperbola is: + // In local coordinate system, value() of hyperbola is: //(a*cosh(u), b*sinh(u)) - //In global, it is (vector formula): - //center + a_vec*cosh(u) + b_vec*sinh(u). - //That's what is being computed here. + // In global, it is (vector formula): + // center + a_vec*cosh(u) + b_vec*sinh(u). + // That's what is being computed here. // DeriVector2 c(this->center, derivparam); @@ -450,40 +500,53 @@ DeriVector2 Hyperbola::Value(double u, double du, const double* derivparam) cons DeriVector2 emaj = f1.subtr(c).getNormalized(); DeriVector2 emin = emaj.rotate90ccw(); double b, db; - b = *(this->radmin); db = this->radmin==derivparam ? 1.0 : 0.0; + b = *(this->radmin); + db = this->radmin == derivparam ? 1.0 : 0.0; double a, da; - a = this->getRadMaj(c,f1,b,db,da); - DeriVector2 a_vec = emaj.multD(a,da); - DeriVector2 b_vec = emin.multD(b,db); + a = this->getRadMaj(c, f1, b, db, da); + DeriVector2 a_vec = emaj.multD(a, da); + DeriVector2 b_vec = emin.multD(b, db); // // sinh, cosh with derivatives: double co, dco, si, dsi; - co = std::cosh(u); dco = std::sinh(u)*du; - si = std::sinh(u); dsi = std::cosh(u)*du; + co = std::cosh(u); + dco = std::sinh(u) * du; + si = std::sinh(u); + dsi = std::cosh(u) * du; - DeriVector2 ret; //point of hyperbola at parameter value of u, in global coordinates - ret = a_vec.multD(co,dco).sum(b_vec.multD(si,dsi)).sum(c); + DeriVector2 ret;// point of hyperbola at parameter value of u, in global coordinates + ret = a_vec.multD(co, dco).sum(b_vec.multD(si, dsi)).sum(c); return ret; } -int Hyperbola::PushOwnParams(VEC_pD &pvec) +int Hyperbola::PushOwnParams(VEC_pD& pvec) { - int cnt=0; - pvec.push_back(center.x); cnt++; - pvec.push_back(center.y); cnt++; - pvec.push_back(focus1.x); cnt++; - pvec.push_back(focus1.y); cnt++; - pvec.push_back(radmin); cnt++; + int cnt = 0; + pvec.push_back(center.x); + cnt++; + pvec.push_back(center.y); + cnt++; + pvec.push_back(focus1.x); + cnt++; + pvec.push_back(focus1.y); + cnt++; + pvec.push_back(radmin); + cnt++; return cnt; } -void Hyperbola::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void Hyperbola::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - center.x=pvec[cnt]; cnt++; - center.y=pvec[cnt]; cnt++; - focus1.x=pvec[cnt]; cnt++; - focus1.y=pvec[cnt]; cnt++; - radmin=pvec[cnt]; cnt++; + center.x = pvec[cnt]; + cnt++; + center.y = pvec[cnt]; + cnt++; + focus1.x = pvec[cnt]; + cnt++; + focus1.y = pvec[cnt]; + cnt++; + radmin = pvec[cnt]; + cnt++; } Hyperbola* Hyperbola::Copy() { @@ -492,28 +555,39 @@ Hyperbola* Hyperbola::Copy() } //--------------- arc of hyperbola -int ArcOfHyperbola::PushOwnParams(VEC_pD &pvec) +int ArcOfHyperbola::PushOwnParams(VEC_pD& pvec) { - int cnt=0; + int cnt = 0; cnt += Hyperbola::PushOwnParams(pvec); - pvec.push_back(start.x); cnt++; - pvec.push_back(start.y); cnt++; - pvec.push_back(end.x); cnt++; - pvec.push_back(end.y); cnt++; - pvec.push_back(startAngle); cnt++; - pvec.push_back(endAngle); cnt++; + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; + pvec.push_back(startAngle); + cnt++; + pvec.push_back(endAngle); + cnt++; return cnt; - } -void ArcOfHyperbola::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void ArcOfHyperbola::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - Hyperbola::ReconstructOnNewPvec(pvec,cnt); - start.x=pvec[cnt]; cnt++; - start.y=pvec[cnt]; cnt++; - end.x=pvec[cnt]; cnt++; - end.y=pvec[cnt]; cnt++; - startAngle=pvec[cnt]; cnt++; - endAngle=pvec[cnt]; cnt++; + Hyperbola::ReconstructOnNewPvec(pvec, cnt); + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; + startAngle = pvec[cnt]; + cnt++; + endAngle = pvec[cnt]; + cnt++; } ArcOfHyperbola* ArcOfHyperbola::Copy() { @@ -543,49 +617,57 @@ DeriVector2 Parabola::CalculateNormal(const Point& p, const double* derivparam) DeriVector2 Parabola::Value(double u, double du, const double* derivparam) const { - //In local coordinate system, value() of parabola is: - //P(U) = O + U*U/(4.*F)*XDir + U*YDir + // In local coordinate system, value() of parabola is: + // P(U) = O + U*U/(4.*F)*XDir + U*YDir DeriVector2 c(this->vertex, derivparam); DeriVector2 f1(this->focus1, derivparam); DeriVector2 fv = f1.subtr(c); - double f,df; + double f, df; f = fv.length(df); DeriVector2 xdir = fv.getNormalized(); DeriVector2 ydir = xdir.rotate90ccw(); - DeriVector2 dirx = xdir.multD(u,du).multD(u,du).divD(4*f,4*df); - DeriVector2 diry = ydir.multD(u,du); + DeriVector2 dirx = xdir.multD(u, du).multD(u, du).divD(4 * f, 4 * df); + DeriVector2 diry = ydir.multD(u, du); DeriVector2 dir = dirx.sum(diry); - DeriVector2 ret; //point of parabola at parameter value of u, in global coordinates + DeriVector2 ret;// point of parabola at parameter value of u, in global coordinates - ret = c.sum( dir ); + ret = c.sum(dir); return ret; } -int Parabola::PushOwnParams(VEC_pD &pvec) +int Parabola::PushOwnParams(VEC_pD& pvec) { - int cnt=0; - pvec.push_back(vertex.x); cnt++; - pvec.push_back(vertex.y); cnt++; - pvec.push_back(focus1.x); cnt++; - pvec.push_back(focus1.y); cnt++; + int cnt = 0; + pvec.push_back(vertex.x); + cnt++; + pvec.push_back(vertex.y); + cnt++; + pvec.push_back(focus1.x); + cnt++; + pvec.push_back(focus1.y); + cnt++; return cnt; } -void Parabola::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void Parabola::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - vertex.x=pvec[cnt]; cnt++; - vertex.y=pvec[cnt]; cnt++; - focus1.x=pvec[cnt]; cnt++; - focus1.y=pvec[cnt]; cnt++; + vertex.x = pvec[cnt]; + cnt++; + vertex.y = pvec[cnt]; + cnt++; + focus1.x = pvec[cnt]; + cnt++; + focus1.y = pvec[cnt]; + cnt++; } Parabola* Parabola::Copy() @@ -595,28 +677,39 @@ Parabola* Parabola::Copy() } //--------------- arc of hyperbola -int ArcOfParabola::PushOwnParams(VEC_pD &pvec) +int ArcOfParabola::PushOwnParams(VEC_pD& pvec) { - int cnt=0; + int cnt = 0; cnt += Parabola::PushOwnParams(pvec); - pvec.push_back(start.x); cnt++; - pvec.push_back(start.y); cnt++; - pvec.push_back(end.x); cnt++; - pvec.push_back(end.y); cnt++; - pvec.push_back(startAngle); cnt++; - pvec.push_back(endAngle); cnt++; + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; + pvec.push_back(startAngle); + cnt++; + pvec.push_back(endAngle); + cnt++; return cnt; - } -void ArcOfParabola::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void ArcOfParabola::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - Parabola::ReconstructOnNewPvec(pvec,cnt); - start.x=pvec[cnt]; cnt++; - start.y=pvec[cnt]; cnt++; - end.x=pvec[cnt]; cnt++; - end.y=pvec[cnt]; cnt++; - startAngle=pvec[cnt]; cnt++; - endAngle=pvec[cnt]; cnt++; + Parabola::ReconstructOnNewPvec(pvec, cnt); + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; + startAngle = pvec[cnt]; + cnt++; + endAngle = pvec[cnt]; + cnt++; } ArcOfParabola* ArcOfParabola::Copy() { @@ -625,7 +718,7 @@ ArcOfParabola* ArcOfParabola::Copy() } // bspline -DeriVector2 BSpline::CalculateNormal(const Point &p, const double* derivparam) const +DeriVector2 BSpline::CalculateNormal(const Point& p, const double* derivparam) const { // place holder DeriVector2 ret; @@ -636,9 +729,9 @@ DeriVector2 BSpline::CalculateNormal(const Point &p, const double* derivparam) c // // https://forum.freecad.org/viewtopic.php?f=10&t=26312#p209486 - if (mult[0] > degree && mult[mult.size()-1] > degree) { - // if endpoints through end poles - if(*p.x == *start.x && *p.y == *start.y) { + if (mult[0] > degree && mult[mult.size() - 1] > degree) { + // if endpoints through end poles + if (*p.x == *start.x && *p.y == *start.y) { // and you are asking about the normal at start point // then tangency is defined by first to second poles DeriVector2 endpt(this->poles[1], derivparam); @@ -647,21 +740,23 @@ DeriVector2 BSpline::CalculateNormal(const Point &p, const double* derivparam) c DeriVector2 tg = endpt.subtr(spt); ret = tg.rotate90ccw(); } - else if(*p.x == *end.x && *p.y == *end.y) { + else if (*p.x == *end.x && *p.y == *end.y) { // and you are asking about the normal at end point // then tangency is defined by last to last but one poles - DeriVector2 endpt(this->poles[poles.size()-1], derivparam); - DeriVector2 spt(this->poles[poles.size()-2], derivparam); + DeriVector2 endpt(this->poles[poles.size() - 1], derivparam); + DeriVector2 spt(this->poles[poles.size() - 2], derivparam); DeriVector2 tg = endpt.subtr(spt); ret = tg.rotate90ccw(); - } else { - // another point and we have no clue until we implement De Boor + } + else { + // another point and we have no clue until we implement De Boor ret = DeriVector2(); } } else { - // either periodic or abnormal endpoint multiplicity, we have no clue so currently unsupported + // either periodic or abnormal endpoint multiplicity, we have no clue so currently + // unsupported ret = DeriVector2(); } @@ -677,13 +772,13 @@ DeriVector2 BSpline::Value(double /*u*/, double /*du*/, const double* /*derivpar return ret; } -int BSpline::PushOwnParams(VEC_pD &pvec) +int BSpline::PushOwnParams(VEC_pD& pvec) { - std::size_t cnt=0; + std::size_t cnt = 0; - for(VEC_P::const_iterator it = poles.begin(); it != poles.end(); ++it) { - pvec.push_back( (*it).x ); - pvec.push_back( (*it).y ); + for (VEC_P::const_iterator it = poles.begin(); it != poles.end(); ++it) { + pvec.push_back((*it).x); + pvec.push_back((*it).y); } cnt = cnt + poles.size() * 2; @@ -694,33 +789,45 @@ int BSpline::PushOwnParams(VEC_pD &pvec) pvec.insert(pvec.end(), knots.begin(), knots.end()); cnt = cnt + knots.size(); - pvec.push_back(start.x); cnt++; - pvec.push_back(start.y); cnt++; - pvec.push_back(end.x); cnt++; - pvec.push_back(end.y); cnt++; + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; return static_cast(cnt); } -void BSpline::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +void BSpline::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) { - for(VEC_P::iterator it = poles.begin(); it != poles.end(); ++it) { - (*it).x = pvec[cnt]; cnt++; - (*it).y = pvec[cnt]; cnt++; + for (VEC_P::iterator it = poles.begin(); it != poles.end(); ++it) { + (*it).x = pvec[cnt]; + cnt++; + (*it).y = pvec[cnt]; + cnt++; } - for(VEC_pD::iterator it = weights.begin(); it != weights.end(); ++it) { - (*it) = pvec[cnt]; cnt++; + for (VEC_pD::iterator it = weights.begin(); it != weights.end(); ++it) { + (*it) = pvec[cnt]; + cnt++; } - for(VEC_pD::iterator it = knots.begin(); it != knots.end(); ++it) { - (*it) = pvec[cnt]; cnt++; + for (VEC_pD::iterator it = knots.begin(); it != knots.end(); ++it) { + (*it) = pvec[cnt]; + cnt++; } - start.x=pvec[cnt]; cnt++; - start.y=pvec[cnt]; cnt++; - end.x=pvec[cnt]; cnt++; - end.y=pvec[cnt]; cnt++; + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; } BSpline* BSpline::Copy() @@ -785,10 +892,10 @@ void BSpline::setupFlattenedKnots() // Adjust for periodic: see OCC documentation for explanation if (periodic) { double period = *knots.back() - *knots.front(); - int c = degree + 1 - mult[0]; // number of knots to pad + int c = degree + 1 - mult[0];// number of knots to pad // Add capacity so that iterators remain valid - flattenedknots.reserve(flattenedknots.size() + 2*c); + flattenedknots.reserve(flattenedknots.size() + 2 * c); // get iterators first for convenience auto frontStart = flattenedknots.end() - mult.back() - c; @@ -810,4 +917,4 @@ void BSpline::setupFlattenedKnots() } } -}//namespace GCS +}// namespace GCS diff --git a/src/Mod/Sketcher/App/planegcs/Geo.h b/src/Mod/Sketcher/App/planegcs/Geo.h index 374d9ca1b5..a66bebe144 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.h +++ b/src/Mod/Sketcher/App/planegcs/Geo.h @@ -30,345 +30,400 @@ namespace GCS { - class Point +class Point +{ +public: + Point() { - public: - Point() - { - x = nullptr; - y = nullptr; - } - Point(double* px, double* py) - { - x = px; - y = py; - } - double* x; - double* y; - }; - - using VEC_P = std::vector; - - ///Class DeriVector2 holds a vector value and its derivative on the - ///parameter that the derivatives are being calculated for now. x,y is the - ///actual vector (v). dx,dy is a derivative of the vector by a parameter - ///(dv/dp). The easiest way to fill the vector in is by passing a point and - ///a derivative parameter pointer to its constructor. x,y are read from the - ///pointers in Point, and dx,dy are set to either 0 or 1 depending on what - ///pointers of Point match the supplied pointer. The derivatives can be set - ///manually as well. The class also provides a bunch of methods to do math - ///on it (and derivatives are calculated implicitly). - /// - class DeriVector2 + x = nullptr; + y = nullptr; + } + Point(double* px, double* py) { - public: - DeriVector2() - { - x = 0; - y = 0; - dx = 0; - dy = 0; - } - DeriVector2(double x, double y) - { - this->x = x; - this->y = y; - this->dx = 0; - this->dy = 0; - } - DeriVector2(double x, double y, double dx, double dy) - { - this->x = x; - this->y = y; - this->dx = dx; - this->dy = dy; - } - DeriVector2(const Point& p, const double* derivparam); - double x, dx; - double y, dy; + x = px; + y = py; + } + double* x; + double* y; +}; - double length() const - { - return sqrt(x * x + y * y); - } - double length(double& dlength) - const;// returns length and writes length deriv into the dlength argument. +using VEC_P = std::vector; - // 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); - } - DeriVector2 subtr(const DeriVector2& v2) const - {// subtracts two vectors and returns result - return DeriVector2(x - v2.x, y - v2.y, dx - v2.dx, dy - v2.dy); - } - DeriVector2 mult(double val) const - { - return DeriVector2(x * val, y * val, dx * val, dy * val); - }// multiplies the vector by a number. Derivatives are scaled. - DeriVector2 multD(double val, double dval) const - {// multiply vector by a variable with a derivative. - return DeriVector2(x * val, y * val, dx * val + x * dval, dy * val + y * dval); - } - DeriVector2 divD(double val, - double dval) const;// divide vector by a variable with a derivative - DeriVector2 rotate90ccw() const - { - return DeriVector2(-y, x, -dy, dx); - } - DeriVector2 rotate90cw() const - { - return DeriVector2(y, -x, dy, -dx); - } - DeriVector2 linCombi(double m1, const DeriVector2& v2, double m2) const - {// linear combination of two vectors - return DeriVector2( - x * m1 + v2.x * m2, y * m1 + v2.y * m2, dx * m1 + v2.dx * m2, dy * m1 + v2.dy * m2); - } - }; - - /////////////////////////////////////// - // Geometries - /////////////////////////////////////// - - class Curve //a base class for all curve-based objects (line, circle/arc, ellipse/arc) +/// Class DeriVector2 holds a vector value and its derivative on the +/// parameter that the derivatives are being calculated for now. x,y is the +/// actual vector (v). dx,dy is a derivative of the vector by a parameter +///(dv/dp). The easiest way to fill the vector in is by passing a point and +/// a derivative parameter pointer to its constructor. x,y are read from the +/// pointers in Point, and dx,dy are set to either 0 or 1 depending on what +/// pointers of Point match the supplied pointer. The derivatives can be set +/// manually as well. The class also provides a bunch of methods to do math +/// on it (and derivatives are calculated implicitly). +/// +class DeriVector2 +{ +public: + DeriVector2() { - public: - virtual ~Curve(){} - //returns normal vector. The vector should point to the left when one - // walks along the curve from start to end. Ellipses and circles are - // assumed to be walked counterclockwise, so the vector should point - // into the shape. - //derivparam is a pointer to a curve parameter (or point coordinate) to - // compute the derivative for. The derivative is returned through dx,dy - // fields of DeriVector2. - virtual DeriVector2 CalculateNormal(const Point &p, const double* derivparam = nullptr) const = 0; - - /** - * @brief Value: returns point (vector) given the value of parameter - * @param u: value of parameter - * @param du: derivative of parameter by derivparam - * @param derivparam: pointer to sketch parameter to calculate the derivative for - * @return - */ - virtual DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const; - - //adds curve's parameters to pvec (used by constraints) - virtual int PushOwnParams(VEC_pD &pvec) = 0; - //recunstruct curve's parameters reading them from pvec starting from index cnt. - //cnt will be incremented by the same value as returned by PushOwnParams() - virtual void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) = 0; - virtual Curve* Copy() = 0; //DeepSOIC: I haven't found a way to simply copy a curve object provided pointer to a curve object. - }; - - class Line: public Curve + x = 0; + y = 0; + dx = 0; + dy = 0; + } + DeriVector2(double x, double y) { - public: - Line(){} - ~Line() override{} - Point p1; - Point p2; - DeriVector2 CalculateNormal(const Point &p, const double* derivparam = nullptr) const override; - DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - Line* Copy() override; - }; - - class Circle: public Curve + this->x = x; + this->y = y; + this->dx = 0; + this->dy = 0; + } + DeriVector2(double x, double y, double dx, double dy) { - public: - Circle(){rad = nullptr;} - ~Circle() override{} - Point center; - double *rad; - DeriVector2 CalculateNormal(const Point &p, const double* derivparam = nullptr) const override; - DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - Circle* Copy() override; - }; + this->x = x; + this->y = y; + this->dx = dx; + this->dy = dy; + } + DeriVector2(const Point& p, const double* derivparam); + double x, dx; + double y, dy; - class Arc: public Circle + double length() const { - public: - Arc(){startAngle=nullptr;endAngle=nullptr;rad=nullptr;} - ~Arc() override{} - double *startAngle; - double *endAngle; - //double *rad; //inherited - Point start; - Point end; - //Point center; //inherited - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - Arc* Copy() override; - }; + return sqrt(x * x + y * y); + } + // returns length and writes length deriv into the dlength argument. + double length(double& dlength) const; - class MajorRadiusConic: public Curve + // 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. + // calculates scalar product of two vectors and returns the result. The derivative + // of the result is written into argument dprd. + double scalarProd(const DeriVector2& v2, double* dprd = nullptr) 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. + double crossProdNorm(const DeriVector2& v2, double& dprd) const; + 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); + } + DeriVector2 subtr(const DeriVector2& v2) const + {// subtracts two vectors and returns result + return DeriVector2(x - v2.x, y - v2.y, dx - v2.dx, dy - v2.dy); + } + DeriVector2 mult(double val) const { - public: - ~MajorRadiusConic() override{} - virtual double getRadMaj(const DeriVector2 ¢er, const DeriVector2 &f1, double b, double db, double &ret_dRadMaj) const = 0; - virtual double getRadMaj(double* derivparam, double &ret_dRadMaj) const = 0; - virtual double getRadMaj() const = 0; - //DeriVector2 CalculateNormal(Point &p, double* derivparam = 0) = 0; - }; - - class Ellipse: public MajorRadiusConic + return DeriVector2(x * val, y * val, dx * val, dy * val); + }// multiplies the vector by a number. Derivatives are scaled. + DeriVector2 multD(double val, double dval) const + {// multiply vector by a variable with a derivative. + return DeriVector2(x * val, y * val, dx * val + x * dval, dy * val + y * dval); + } + // divide vector by a variable with a derivative + DeriVector2 divD(double val, double dval) const; + DeriVector2 rotate90ccw() const { - public: - Ellipse(){ radmin = nullptr;} - ~Ellipse() override{} - Point center; - Point focus1; - double *radmin; - double getRadMaj(const DeriVector2 ¢er, const DeriVector2 &f1, double b, double db, double &ret_dRadMaj) const override; - double getRadMaj(double* derivparam, double &ret_dRadMaj) const override; - double getRadMaj() const override; - DeriVector2 CalculateNormal(const Point &p, const double* derivparam = nullptr) const override; - DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - Ellipse* Copy() override; - }; - - class ArcOfEllipse: public Ellipse + return DeriVector2(-y, x, -dy, dx); + } + DeriVector2 rotate90cw() const { - public: - ArcOfEllipse(){startAngle=nullptr;endAngle=nullptr;radmin = nullptr;} - ~ArcOfEllipse() override{} - double *startAngle; - double *endAngle; - //double *radmin; //inherited - Point start; - Point end; - //Point center; //inherited - //double *focus1.x; //inherited - //double *focus1.y; //inherited - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - ArcOfEllipse* Copy() override; - }; + return DeriVector2(y, -x, dy, -dx); + } + DeriVector2 linCombi(double m1, const DeriVector2& v2, double m2) const + {// linear combination of two vectors + return DeriVector2( + x * m1 + v2.x * m2, y * m1 + v2.y * m2, dx * m1 + v2.dx * m2, dy * m1 + v2.dy * m2); + } +}; - class Hyperbola: public MajorRadiusConic +/////////////////////////////////////// +// Geometries +/////////////////////////////////////// + +class Curve// a base class for all curve-based objects (line, circle/arc, ellipse/arc) +{ +public: + virtual ~Curve() + {} + // returns normal vector. The vector should point to the left when one + // walks along the curve from start to end. Ellipses and circles are + // assumed to be walked counterclockwise, so the vector should point + // into the shape. + // derivparam is a pointer to a curve parameter (or point coordinate) to + // compute the derivative for. The derivative is returned through dx,dy + // fields of DeriVector2. + virtual DeriVector2 CalculateNormal(const Point& p, + const double* derivparam = nullptr) const = 0; + + /** + * @brief Value: returns point (vector) given the value of parameter + * @param u: value of parameter + * @param du: derivative of parameter by derivparam + * @param derivparam: pointer to sketch parameter to calculate the derivative for + * @return + */ + virtual DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const; + + // adds curve's parameters to pvec (used by constraints) + virtual int PushOwnParams(VEC_pD& pvec) = 0; + // recunstruct curve's parameters reading them from pvec starting from index cnt. + // cnt will be incremented by the same value as returned by PushOwnParams() + virtual void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) = 0; + // DeepSOIC: I haven't found a way to simply copy a curve object provided pointer to a curve + // object. + virtual Curve* Copy() = 0; +}; + +class Line: public Curve +{ +public: + Line() + {} + ~Line() override + {} + Point p1; + Point p2; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Line* Copy() override; +}; + +class Circle: public Curve +{ +public: + Circle() { - public: - Hyperbola(){ radmin = nullptr;} - ~Hyperbola() override{} - Point center; - Point focus1; - double *radmin; - double getRadMaj(const DeriVector2 ¢er, const DeriVector2 &f1, double b, double db, double &ret_dRadMaj) const override; - double getRadMaj(double* derivparam, double &ret_dRadMaj) const override; - double getRadMaj() const override; - DeriVector2 CalculateNormal(const Point &p, const double* derivparam = nullptr) const override; - DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - Hyperbola* Copy() override; - }; + rad = nullptr; + } + ~Circle() override + {} + Point center; + double* rad; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Circle* Copy() override; +}; - class ArcOfHyperbola: public Hyperbola +class Arc: public Circle +{ +public: + Arc() { - public: - ArcOfHyperbola(){startAngle=nullptr;endAngle=nullptr;radmin = nullptr;} - ~ArcOfHyperbola() override{} - // parameters - double *startAngle; - double *endAngle; - Point start; - Point end; - // interface helpers - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - ArcOfHyperbola* Copy() override; - }; + startAngle = nullptr; + endAngle = nullptr; + rad = nullptr; + } + ~Arc() override + {} + double* startAngle; + double* endAngle; + // double *rad; //inherited + Point start; + Point end; + // Point center; //inherited + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Arc* Copy() override; +}; - class Parabola: public Curve +class MajorRadiusConic: public Curve +{ +public: + ~MajorRadiusConic() override + {} + virtual double getRadMaj(const DeriVector2& center, const DeriVector2& f1, double b, double db, + double& ret_dRadMaj) const = 0; + virtual double getRadMaj(double* derivparam, double& ret_dRadMaj) const = 0; + virtual double getRadMaj() const = 0; + // DeriVector2 CalculateNormal(Point &p, double* derivparam = 0) = 0; +}; + +class Ellipse: public MajorRadiusConic +{ +public: + Ellipse() { - public: - Parabola(){ } - ~Parabola() override{} - Point vertex; - Point focus1; - DeriVector2 CalculateNormal(const Point &p, const double* derivparam = nullptr) const override; - DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - Parabola* Copy() override; - }; + radmin = nullptr; + } + ~Ellipse() override + {} + Point center; + Point focus1; + double* radmin; + double getRadMaj(const DeriVector2& center, const DeriVector2& f1, double b, double db, + double& ret_dRadMaj) const override; + double getRadMaj(double* derivparam, double& ret_dRadMaj) const override; + double getRadMaj() const override; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Ellipse* Copy() override; +}; - class ArcOfParabola: public Parabola +class ArcOfEllipse: public Ellipse +{ +public: + ArcOfEllipse() { - public: - ArcOfParabola(){startAngle=nullptr;endAngle=nullptr;} - ~ArcOfParabola() override{} - // parameters - double *startAngle; - double *endAngle; - Point start; - Point end; - // interface helpers - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - ArcOfParabola* Copy() override; - }; + startAngle = nullptr; + endAngle = nullptr; + radmin = nullptr; + } + ~ArcOfEllipse() override + {} + double* startAngle; + double* endAngle; + // double *radmin; //inherited + Point start; + Point end; + // Point center; //inherited + // double *focus1.x; //inherited + // double *focus1.y; //inherited + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + ArcOfEllipse* Copy() override; +}; - class BSpline: public Curve +class Hyperbola: public MajorRadiusConic +{ +public: + Hyperbola() { - public: - BSpline(){periodic=false;degree=2;} - ~BSpline() override{} - // parameters - VEC_P poles; // TODO: use better data structures so poles.x and poles.y - VEC_pD weights; - VEC_pD knots; - // dependent parameters (depends on previous parameters, - // but an "arcrules" constraint alike would be required to gain the commodity of simple coincident - // with endpoint constraints) - Point start; - Point end; - // not solver parameters - VEC_I mult; - int degree; - bool periodic; - VEC_I knotpointGeoids; // geoids of knotpoints as to index Geom array - VEC_D flattenedknots; // knot vector with repetitions for multiplicity and "padding" for periodic spline - // interface helpers - DeriVector2 CalculateNormal(const Point &p, const double* derivparam = nullptr) const override; - DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; - int PushOwnParams(VEC_pD &pvec) override; - void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override; - BSpline* Copy() override; - /// finds the value B_i(x) such that spline(x) = sum(poles[i] * B_i(x)) - /// x is the point at which combination is needed - /// k is the range in `flattenedknots` that contains x - /// i is index of control point - /// p is the degree - double getLinCombFactor(double x, size_t k, size_t i, unsigned int p); - inline double getLinCombFactor(double x, size_t k, size_t i) - { return getLinCombFactor(x, k, i, degree); } - void setupFlattenedKnots(); - /// finds spline(x) for the given parameter and knot/pole vector - /// x is the point at which combination is needed - /// k is the range in `flattenedknots` that contains x - /// p is the degree - /// d is the vector of (relevant) poles (this will be changed) - /// flatknots is the vector of knots - static double splineValue(double x, size_t k, unsigned int p, VEC_D& d, const VEC_D& flatknots); - }; + radmin = nullptr; + } + ~Hyperbola() override + {} + Point center; + Point focus1; + double* radmin; + double getRadMaj(const DeriVector2& center, const DeriVector2& f1, double b, double db, + double& ret_dRadMaj) const override; + double getRadMaj(double* derivparam, double& ret_dRadMaj) const override; + double getRadMaj() const override; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Hyperbola* Copy() override; +}; -} //namespace GCS +class ArcOfHyperbola: public Hyperbola +{ +public: + ArcOfHyperbola() + { + startAngle = nullptr; + endAngle = nullptr; + radmin = nullptr; + } + ~ArcOfHyperbola() override + {} + // parameters + double* startAngle; + double* endAngle; + Point start; + Point end; + // interface helpers + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + ArcOfHyperbola* Copy() override; +}; -#endif // PLANEGCS_GEO_H +class Parabola: public Curve +{ +public: + Parabola() + {} + ~Parabola() override + {} + Point vertex; + Point focus1; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Parabola* Copy() override; +}; + +class ArcOfParabola: public Parabola +{ +public: + ArcOfParabola() + { + startAngle = nullptr; + endAngle = nullptr; + } + ~ArcOfParabola() override + {} + // parameters + double* startAngle; + double* endAngle; + Point start; + Point end; + // interface helpers + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + ArcOfParabola* Copy() override; +}; + +class BSpline: public Curve +{ +public: + BSpline() + { + periodic = false; + degree = 2; + } + ~BSpline() override + {} + // parameters + VEC_P poles;// TODO: use better data structures so poles.x and poles.y + VEC_pD weights; + VEC_pD knots; + // dependent parameters (depends on previous parameters, + // but an "arcrules" constraint alike would be required to gain the commodity of simple + // coincident with endpoint constraints) + Point start; + Point end; + // not solver parameters + VEC_I mult; + int degree; + bool periodic; + VEC_I knotpointGeoids;// geoids of knotpoints as to index Geom array + // knot vector with repetitions for multiplicity and "padding" for periodic spline + // interface helpers + VEC_D flattenedknots; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + BSpline* Copy() override; + /// finds the value B_i(x) such that spline(x) = sum(poles[i] * B_i(x)) + /// x is the point at which combination is needed + /// k is the range in `flattenedknots` that contains x + /// i is index of control point + /// p is the degree + double getLinCombFactor(double x, size_t k, size_t i, unsigned int p); + inline double getLinCombFactor(double x, size_t k, size_t i) + { + return getLinCombFactor(x, k, i, degree); + } + void setupFlattenedKnots(); + /// finds spline(x) for the given parameter and knot/pole vector + /// x is the point at which combination is needed + /// k is the range in `flattenedknots` that contains x + /// p is the degree + /// d is the vector of (relevant) poles (this will be changed) + /// flatknots is the vector of knots + static double splineValue(double x, size_t k, unsigned int p, VEC_D& d, const VEC_D& flatknots); +}; + +}// namespace GCS + +#endif// PLANEGCS_GEO_H diff --git a/src/Mod/Sketcher/App/planegcs/SubSystem.cpp b/src/Mod/Sketcher/App/planegcs/SubSystem.cpp index a2622a75d3..f6e5b0db46 100644 --- a/src/Mod/Sketcher/App/planegcs/SubSystem.cpp +++ b/src/Mod/Sketcher/App/planegcs/SubSystem.cpp @@ -30,25 +30,23 @@ namespace GCS { // SubSystem -SubSystem::SubSystem(std::vector &clist_, VEC_pD ¶ms) -: clist(clist_) +SubSystem::SubSystem(std::vector& clist_, VEC_pD& params) + : clist(clist_) { MAP_pD_pD dummymap; initialize(params, dummymap); } -SubSystem::SubSystem(std::vector &clist_, VEC_pD ¶ms, - MAP_pD_pD &reductionmap) -: clist(clist_) +SubSystem::SubSystem(std::vector& clist_, VEC_pD& params, MAP_pD_pD& reductionmap) + : clist(clist_) { initialize(params, reductionmap); } SubSystem::~SubSystem() -{ -} +{} -void SubSystem::initialize(VEC_pD ¶ms, MAP_pD_pD &reductionmap) +void SubSystem::initialize(VEC_pD& params, MAP_pD_pD& reductionmap) { csize = static_cast(clist.size()); @@ -101,146 +99,141 @@ void SubSystem::initialize(VEC_pD ¶ms, MAP_pD_pD &reductionmap) psize = static_cast(plist.size()); pvals.resize(psize); pmap.clear(); - for (int j=0; j < psize; j++) { + for (int j = 0; j < psize; j++) { pmap[plist[j]] = &pvals[j]; pvals[j] = *plist[j]; } - for (MAP_pD_I::const_iterator itr=rindex.begin(); itr != rindex.end(); ++itr) + for (MAP_pD_I::const_iterator itr = rindex.begin(); itr != rindex.end(); ++itr) pmap[itr->first] = &pvals[itr->second]; c2p.clear(); p2c.clear(); - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) { - (*constr)->revertParams(); // ensure that the constraint points to the original parameters + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + (*constr)->revertParams();// ensure that the constraint points to the original parameters VEC_pD constr_params_orig = (*constr)->params(); SET_pD constr_params; - for (VEC_pD::const_iterator p=constr_params_orig.begin(); - p != constr_params_orig.end(); ++p) { + for (VEC_pD::const_iterator p = constr_params_orig.begin(); p != constr_params_orig.end(); + ++p) { MAP_pD_pD::const_iterator pmapfind = pmap.find(*p); if (pmapfind != pmap.end()) constr_params.insert(pmapfind->second); } - for (SET_pD::const_iterator p=constr_params.begin(); - p != constr_params.end(); ++p) { -// jacobi.set(*constr, *p, 0.); + for (SET_pD::const_iterator p = constr_params.begin(); p != constr_params.end(); ++p) { + // jacobi.set(*constr, *p, 0.); c2p[*constr].push_back(*p); p2c[*p].push_back(*constr); } -// (*constr)->redirectParams(pmap); // redirect parameters to pvec + // (*constr)->redirectParams(pmap); // redirect parameters to pvec } } void SubSystem::redirectParams() { // copying values to pvals - for (MAP_pD_pD::const_iterator p=pmap.begin(); - p != pmap.end(); ++p) + for (MAP_pD_pD::const_iterator p = pmap.begin(); p != pmap.end(); ++p) *(p->second) = *(p->first); // redirect constraints to point to pvals - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) { - (*constr)->revertParams(); // this line will normally not be necessary + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + (*constr)->revertParams();// this line will normally not be necessary (*constr)->redirectParams(pmap); } } void SubSystem::revertParams() { - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); ++constr) (*constr)->revertParams(); } -void SubSystem::getParamMap(MAP_pD_pD &pmapOut) +void SubSystem::getParamMap(MAP_pD_pD& pmapOut) { pmapOut = pmap; } -void SubSystem::getParamList(VEC_pD &plistOut) +void SubSystem::getParamList(VEC_pD& plistOut) { plistOut = plist; } -void SubSystem::getParams(VEC_pD ¶ms, Eigen::VectorXd &xOut) +void SubSystem::getParams(VEC_pD& params, Eigen::VectorXd& xOut) { if (xOut.size() != int(params.size())) xOut.setZero(params.size()); - for (int j=0; j < int(params.size()); j++) { - MAP_pD_pD::const_iterator - pmapfind = pmap.find(params[j]); + for (int j = 0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); if (pmapfind != pmap.end()) xOut[j] = *(pmapfind->second); } } -void SubSystem::getParams(Eigen::VectorXd &xOut) +void SubSystem::getParams(Eigen::VectorXd& xOut) { if (xOut.size() != psize) xOut.setZero(psize); - for (int i=0; i < psize; i++) + for (int i = 0; i < psize; i++) xOut[i] = pvals[i]; } -void SubSystem::setParams(VEC_pD ¶ms, Eigen::VectorXd &xIn) +void SubSystem::setParams(VEC_pD& params, Eigen::VectorXd& xIn) { assert(xIn.size() == int(params.size())); - for (int j=0; j < int(params.size()); j++) { - MAP_pD_pD::const_iterator - pmapfind = pmap.find(params[j]); + for (int j = 0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); if (pmapfind != pmap.end()) *(pmapfind->second) = xIn[j]; } } -void SubSystem::setParams(Eigen::VectorXd &xIn) +void SubSystem::setParams(Eigen::VectorXd& xIn) { assert(xIn.size() == psize); - for (int i=0; i < psize; i++) + for (int i = 0; i < psize; i++) pvals[i] = xIn[i]; } -void SubSystem::getConstraintList(std::vector &clist_) +void SubSystem::getConstraintList(std::vector& clist_) { - clist_= clist; + clist_ = clist; } double SubSystem::error() { double err = 0.; - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr) { + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr) { double tmp = (*constr)->error(); - err += tmp*tmp; + err += tmp * tmp; } err *= 0.5; return err; } -void SubSystem::calcResidual(Eigen::VectorXd &r) +void SubSystem::calcResidual(Eigen::VectorXd& r) { assert(r.size() == csize); - int i=0; - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr, i++) { + int i = 0; + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr, i++) { r[i] = (*constr)->error(); } } -void SubSystem::calcResidual(Eigen::VectorXd &r, double &err) +void SubSystem::calcResidual(Eigen::VectorXd& r, double& err) { assert(r.size() == csize); - int i=0; + int i = 0; err = 0.; - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr, i++) { + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr, i++) { r[i] = (*constr)->error(); - err += r[i]*r[i]; + err += r[i] * r[i]; } err *= 0.5; } @@ -261,74 +254,71 @@ void SubSystem::calcJacobi() } */ -void SubSystem::calcJacobi(VEC_pD ¶ms, Eigen::MatrixXd &jacobi) +void SubSystem::calcJacobi(VEC_pD& params, Eigen::MatrixXd& jacobi) { jacobi.setZero(csize, params.size()); - for (int j=0; j < int(params.size()); j++) { - MAP_pD_pD::const_iterator - pmapfind = pmap.find(params[j]); + for (int j = 0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); if (pmapfind != pmap.end()) - for (int i=0; i < csize; i++) - jacobi(i,j) = clist[i]->grad(pmapfind->second); + for (int i = 0; i < csize; i++) + jacobi(i, j) = clist[i]->grad(pmapfind->second); } } -void SubSystem::calcJacobi(Eigen::MatrixXd &jacobi) +void SubSystem::calcJacobi(Eigen::MatrixXd& jacobi) { calcJacobi(plist, jacobi); } -void SubSystem::calcGrad(VEC_pD ¶ms, Eigen::VectorXd &grad) +void SubSystem::calcGrad(VEC_pD& params, Eigen::VectorXd& grad) { assert(grad.size() == int(params.size())); grad.setZero(); - for (int j=0; j < int(params.size()); j++) { - MAP_pD_pD::const_iterator - pmapfind = pmap.find(params[j]); + for (int j = 0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); if (pmapfind != pmap.end()) { // assert(p2c.find(pmapfind->second) != p2c.end()); - std::vector constrs=p2c[pmapfind->second]; - for (std::vector::const_iterator constr = constrs.begin(); - constr != constrs.end(); ++constr) + std::vector constrs = p2c[pmapfind->second]; + for (std::vector::const_iterator constr = constrs.begin(); + constr != constrs.end(); + ++constr) grad[j] += (*constr)->error() * (*constr)->grad(pmapfind->second); } } } -void SubSystem::calcGrad(Eigen::VectorXd &grad) +void SubSystem::calcGrad(Eigen::VectorXd& grad) { calcGrad(plist, grad); } -double SubSystem::maxStep(VEC_pD ¶ms, Eigen::VectorXd &xdir) +double SubSystem::maxStep(VEC_pD& params, Eigen::VectorXd& xdir) { assert(xdir.size() == int(params.size())); MAP_pD_D dir; - for (int j=0; j < int(params.size()); j++) { + for (int j = 0; j < int(params.size()); j++) { MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); if (pmapfind != pmap.end()) dir[pmapfind->second] = xdir[j]; } - double alpha=1e10; - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) + double alpha = 1e10; + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); ++constr) alpha = (*constr)->maxStep(dir, alpha); return alpha; } -double SubSystem::maxStep(Eigen::VectorXd &xdir) +double SubSystem::maxStep(Eigen::VectorXd& xdir) { return maxStep(plist, xdir); } void SubSystem::applySolution() { - for (MAP_pD_pD::const_iterator it=pmap.begin(); - it != pmap.end(); ++it) + for (MAP_pD_pD::const_iterator it = pmap.begin(); it != pmap.end(); ++it) *(it->first) = *(it->second); } @@ -336,18 +326,17 @@ void SubSystem::analyse(Eigen::MatrixXd& /*J*/, Eigen::MatrixXd& /*ker*/, Eigen: {} void SubSystem::report() -{ -} +{} void SubSystem::printResidual() { Eigen::VectorXd r(csize); - int i=0; + int i = 0; double err = 0.; - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr, i++) { + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr, i++) { r[i] = (*constr)->error(); - err += r[i]*r[i]; + err += r[i] * r[i]; } err *= 0.5; std::cout << "Residual r = " << r.transpose() << std::endl; @@ -355,4 +344,4 @@ void SubSystem::printResidual() } -} //namespace GCS +}// namespace GCS diff --git a/src/Mod/Sketcher/App/planegcs/SubSystem.h b/src/Mod/Sketcher/App/planegcs/SubSystem.h index 0ff125da95..5c0bafebb5 100644 --- a/src/Mod/Sketcher/App/planegcs/SubSystem.h +++ b/src/Mod/Sketcher/App/planegcs/SubSystem.h @@ -34,60 +34,65 @@ namespace GCS { - class SubSystem +class SubSystem +{ +private: + int psize, csize; + std::vector clist; + VEC_pD plist; // pointers to the original parameters + MAP_pD_pD pmap;// redirection map from the original parameters to pvals + VEC_D pvals; // current variables vector (psize) + // JacobianMatrix jacobi; // jacobi matrix of the residuals + std::map c2p; // constraint to parameter adjacency list + std::map> p2c;// parameter to constraint adjacency list + void initialize(VEC_pD& params, MAP_pD_pD& reductionmap);// called by the constructors +public: + SubSystem(std::vector& clist_, VEC_pD& params); + SubSystem(std::vector& clist_, VEC_pD& params, MAP_pD_pD& reductionmap); + ~SubSystem(); + + int pSize() { - private: - int psize, csize; - std::vector clist; - VEC_pD plist; // pointers to the original parameters - MAP_pD_pD pmap; // redirection map from the original parameters to pvals - VEC_D pvals; // current variables vector (psize) -// JacobianMatrix jacobi; // jacobi matrix of the residuals - std::map c2p; // constraint to parameter adjacency list - std::map > p2c; // parameter to constraint adjacency list - void initialize(VEC_pD ¶ms, MAP_pD_pD &reductionmap); // called by the constructors - public: - SubSystem(std::vector &clist_, VEC_pD ¶ms); - SubSystem(std::vector &clist_, VEC_pD ¶ms, - MAP_pD_pD &reductionmap); - ~SubSystem(); - - int pSize() { return psize; }; - int cSize() { return csize; }; - - void redirectParams(); - void revertParams(); - - void getParamMap(MAP_pD_pD &pmapOut); - void getParamList(VEC_pD &plistOut); - - void getParams(VEC_pD ¶ms, Eigen::VectorXd &xOut); - void getParams(Eigen::VectorXd &xOut); - void setParams(VEC_pD ¶ms, Eigen::VectorXd &xIn); - void setParams(Eigen::VectorXd &xIn); - - void getConstraintList(std::vector &clist_); - - double error(); - void calcResidual(Eigen::VectorXd &r); - void calcResidual(Eigen::VectorXd &r, double &err); - void calcJacobi(VEC_pD ¶ms, Eigen::MatrixXd &jacobi); - void calcJacobi(Eigen::MatrixXd &jacobi); - void calcGrad(VEC_pD ¶ms, Eigen::VectorXd &grad); - void calcGrad(Eigen::VectorXd &grad); - - double maxStep(VEC_pD ¶ms, Eigen::VectorXd &xdir); - double maxStep(Eigen::VectorXd &xdir); - - void applySolution(); - void analyse(Eigen::MatrixXd &J, Eigen::MatrixXd &ker, Eigen::MatrixXd &img); - void report(); - - void printResidual(); + return psize; + }; + int cSize() + { + return csize; }; - double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir); + void redirectParams(); + void revertParams(); -} //namespace GCS + void getParamMap(MAP_pD_pD& pmapOut); + void getParamList(VEC_pD& plistOut); -#endif // PLANEGCS_SUBSYSTEM_H + void getParams(VEC_pD& params, Eigen::VectorXd& xOut); + void getParams(Eigen::VectorXd& xOut); + void setParams(VEC_pD& params, Eigen::VectorXd& xIn); + void setParams(Eigen::VectorXd& xIn); + + void getConstraintList(std::vector& clist_); + + double error(); + void calcResidual(Eigen::VectorXd& r); + void calcResidual(Eigen::VectorXd& r, double& err); + void calcJacobi(VEC_pD& params, Eigen::MatrixXd& jacobi); + void calcJacobi(Eigen::MatrixXd& jacobi); + void calcGrad(VEC_pD& params, Eigen::VectorXd& grad); + void calcGrad(Eigen::VectorXd& grad); + + double maxStep(VEC_pD& params, Eigen::VectorXd& xdir); + double maxStep(Eigen::VectorXd& xdir); + + void applySolution(); + void analyse(Eigen::MatrixXd& J, Eigen::MatrixXd& ker, Eigen::MatrixXd& img); + void report(); + + void printResidual(); +}; + +double lineSearch(SubSystem* subsys, Eigen::VectorXd& xdir); + +}// namespace GCS + +#endif// PLANEGCS_SUBSYSTEM_H diff --git a/src/Mod/Sketcher/App/planegcs/Util.h b/src/Mod/Sketcher/App/planegcs/Util.h index f0faad876b..3aa0782b41 100644 --- a/src/Mod/Sketcher/App/planegcs/Util.h +++ b/src/Mod/Sketcher/App/planegcs/Util.h @@ -30,19 +30,19 @@ namespace GCS { - using VEC_pD = std::vector; - using VEC_D = std::vector; - using VEC_I = std::vector; - using MAP_pD_pD = std::map; - using MAP_pD_D = std::map; - using MAP_pD_I = std::map; - using SET_pD = std::set; - using SET_I = std::set; +using VEC_pD = std::vector; +using VEC_D = std::vector; +using VEC_I = std::vector; +using MAP_pD_pD = std::map; +using MAP_pD_D = std::map; +using MAP_pD_I = std::map; +using SET_pD = std::set; +using SET_I = std::set; #ifndef M_PI #define M_PI 3.14159265358979323846 #endif -} //namespace GCS +}// namespace GCS -#endif // PLANEGCS_UTIL_H +#endif// PLANEGCS_UTIL_H diff --git a/src/Mod/Sketcher/App/planegcs/qp_eq.cpp b/src/Mod/Sketcher/App/planegcs/qp_eq.cpp index 7eeb58fafc..9673915316 100644 --- a/src/Mod/Sketcher/App/planegcs/qp_eq.cpp +++ b/src/Mod/Sketcher/App/planegcs/qp_eq.cpp @@ -24,18 +24,17 @@ #pragma warning(disable : 4244) #endif -#include #include +#include using namespace Eigen; // minimizes ( 0.5 * x^T * H * x + g^T * x ) under the condition ( A*x + c = 0 ) // it returns the solution in x, the row-space of A in Y, and the null space of A in Z -int qp_eq(MatrixXd &H, VectorXd &g, MatrixXd &A, VectorXd &c, - VectorXd &x, MatrixXd &Y, MatrixXd &Z) +int qp_eq(MatrixXd& H, VectorXd& g, MatrixXd& A, VectorXd& c, VectorXd& x, MatrixXd& Y, MatrixXd& Z) { FullPivHouseholderQR qrAT(A.transpose()); - MatrixXd Q = qrAT.matrixQ (); + MatrixXd Q = qrAT.matrixQ(); size_t params_num = qrAT.rows(); size_t constr_num = qrAT.cols(); @@ -48,22 +47,23 @@ int qp_eq(MatrixXd &H, VectorXd &g, MatrixXd &A, VectorXd &c, // Q = [Q1,Q2], R=[R1;0] // Y = Q1 * inv(R^T) * P^T // Z = Q2 - Y = qrAT.matrixQR().topRows(constr_num) - .triangularView() - .transpose() - .solve(Q.leftCols(rank)) + Y = qrAT.matrixQR() + .topRows(constr_num) + .triangularView() + .transpose() + .solve(Q.leftCols(rank)) * qrAT.colsPermutation().transpose(); if (params_num == rank) - x = - Y * c; + x = -Y * c; else { - Z = Q.rightCols(params_num-rank); + Z = Q.rightCols(params_num - rank); MatrixXd ZTHZ = Z.transpose() * H * Z; VectorXd rhs = Z.transpose() * (H * Y * c - g); VectorXd y = ZTHZ.colPivHouseholderQr().solve(rhs); - x = - Y * c + Z * y; + x = -Y * c + Z * y; } return 0; diff --git a/src/Mod/Sketcher/App/planegcs/qp_eq.h b/src/Mod/Sketcher/App/planegcs/qp_eq.h index 8c163fd9d1..0d5489250e 100644 --- a/src/Mod/Sketcher/App/planegcs/qp_eq.h +++ b/src/Mod/Sketcher/App/planegcs/qp_eq.h @@ -21,5 +21,5 @@ ***************************************************************************/ #include -int qp_eq(Eigen::MatrixXd &H, Eigen::VectorXd &g, Eigen::MatrixXd &A, Eigen::VectorXd &c, - Eigen::VectorXd &x, Eigen::MatrixXd &Y, Eigen::MatrixXd &Z); +int qp_eq(Eigen::MatrixXd& H, Eigen::VectorXd& g, Eigen::MatrixXd& A, Eigen::VectorXd& c, + Eigen::VectorXd& x, Eigen::MatrixXd& Y, Eigen::MatrixXd& Z);