From 858f4cb85b4b3ff8a36027691b0cc1925f70e5e1 Mon Sep 17 00:00:00 2001 From: Abdullah Tahiri Date: Sun, 4 Mar 2018 16:24:59 +0100 Subject: [PATCH] GCS: Full detection of dependent parameters with Dense Full Household pivoting QR decomposition - Improve Debug information. - Support for addition of constraints with driving information for the solver constraints. - Removal of the driven constraints from the Jacobian for QR decomposition - Removal of the value freewheeling parameter from the Jacobian for QR decomposition - Full detection of dependent parameters where DoF are present for Dense QR. --- src/Mod/Sketcher/App/planegcs/GCS.cpp | 623 ++++++++++++++++---------- src/Mod/Sketcher/App/planegcs/GCS.h | 142 +++--- 2 files changed, 458 insertions(+), 307 deletions(-) diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index 6dc879e21e..3f1d517c78 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -26,12 +26,15 @@ //#define _GCS_DEBUG //#define _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX +//#define _DEBUG_TO_FILE // Many matrices surpass the report view string size. #undef _GCS_DEBUG #undef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX +#undef _DEBUG_TO_FILE // 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(StreamPrecision,0,",",",\n","[","]","[","]") +#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(3,0,",",",\n","[","]","[","]") /* Parameters: * * StreamPrecision, @@ -58,8 +61,16 @@ // NOTE2: solved in eigen3.3 #define EIGEN_VERSION (EIGEN_WORLD_VERSION * 10000 \ - + EIGEN_MAJOR_VERSION * 100 \ - + EIGEN_MINOR_VERSION) ++ EIGEN_MAJOR_VERSION * 100 \ ++ EIGEN_MINOR_VERSION) + +// Extraction of Q matrix for Debugging used to crash +#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX +#if EIGEN_VERSION >= 30304 +#define SPARSE_Q_MATRIX +#endif +#endif + #if EIGEN_VERSION >= 30202 #define EIGEN_SPARSEQR_COMPATIBLE @@ -81,7 +92,8 @@ #include #endif -#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ // this is to be enabled in Constraints.h when needed. +// _GCS_EXTRACT_SOLVER_SUBSYSTEM_ to be enabled in Constraints.h when needed. +#if defined(_GCS_EXTRACT_SOLVER_SUBSYSTEM_) || defined(_DEBUG_TO_FILE) #include #define CASE_NOT_IMP(X) case X: { subsystemfile << "//" #X "not yet implemented" << std::endl; break; } @@ -99,33 +111,77 @@ typedef Eigen::PermutationMatrix PermutationType; #ifdef _GCS_DEBUG void LogMatrix(std::string str, Eigen::MatrixXd matrix ) { +#ifdef _DEBUG_TO_FILE + std::ofstream stream; + stream.open("GCS_debug.txt", std::ofstream::out | std::ofstream::app); +#else // Debug code starts std::stringstream stream; +#endif + stream << '\n' << " " << str << " =" << '\n'; stream << "[" << '\n'; stream << matrix << '\n' ; stream << "]" << '\n'; +#ifdef _DEBUG_TO_FILE + stream.flush(); + stream.close(); +#else const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); +#endif } void LogMatrix(std::string str, MatrixIndexType matrix ) { + #ifdef _DEBUG_TO_FILE + std::ofstream stream; + stream.open("GCS_debug.txt", std::ofstream::out | std::ofstream::app); + #else // Debug code starts std::stringstream stream; + #endif + stream << '\n' << " " << str << " =" << '\n'; stream << "[" << '\n'; stream << matrix << '\n' ; stream << "]" << '\n'; - + + #ifdef _DEBUG_TO_FILE + stream.flush(); + stream.close(); + #else const std::string tmp = stream.str(); - + Base::Console().Log(tmp.c_str()); + #endif } #endif +void LogString(std::string str) +{ + #ifdef _DEBUG_TO_FILE + std::ofstream stream; + stream.open("GCS_debug.txt", std::ofstream::out | std::ofstream::app); + #else + // Debug code starts + std::stringstream stream; + #endif + + stream << str << " =" << std::endl; + + #ifdef _DEBUG_TO_FILE + stream.flush(); + stream.close(); + #else + const std::string tmp = stream.str(); + + Base::Console().Log(tmp.c_str()); + #endif +} + #ifndef EIGEN_STOCK_FULLPIVLU_COMPUTE namespace Eigen { @@ -239,6 +295,7 @@ typedef boost::adjacency_list Gra // System System::System() : plist(0) + , pdrivenlist(0) , pdependentparameters(0) , clist(0) , c2p() @@ -360,6 +417,7 @@ System::~System() void System::clear() { plist.clear(); + pdrivenlist.clear(); pIndex.clear(); pdependentparameters.clear(); hasUnknowns = false; @@ -435,445 +493,475 @@ void System::removeConstraint(Constraint *constr) // basic constraints -int System::addConstraintEqual(double *param1, double *param2, int tagId) +int System::addConstraintEqual(double *param1, double *param2, int tagId, bool driving) { Constraint *constr = new ConstraintEqual(param1, param2); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } int System::addConstraintDifference(double *param1, double *param2, - double *difference, int tagId) + double *difference, int tagId, bool driving) { Constraint *constr = new ConstraintDifference(param1, param2, difference); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintP2PDistance(Point &p1, Point &p2, double *distance, int tagId) +int System::addConstraintP2PDistance(Point &p1, Point &p2, double *distance, int tagId, bool driving) { Constraint *constr = new ConstraintP2PDistance(p1, p2, distance); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } int System::addConstraintP2PAngle(Point &p1, Point &p2, double *angle, - double incrAngle, int tagId) + double incrAngle, int tagId, bool driving) { 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*/) +int System::addConstraintP2PAngle(Point &p1, Point &p2, double *angle, int /*tagId*/, bool driving) { - return addConstraintP2PAngle(p1, p2, angle, 0.); + return addConstraintP2PAngle(p1, p2, angle, 0., 0, driving); } -int System::addConstraintP2LDistance(Point &p, Line &l, double *distance, int tagId) +int System::addConstraintP2LDistance(Point &p, Line &l, double *distance, int tagId, bool driving) { 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) +int System::addConstraintPointOnLine(Point &p, Line &l, int tagId, bool driving) { 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) +int System::addConstraintPointOnLine(Point &p, Point &lp1, Point &lp2, int tagId, bool driving) { 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) +int System::addConstraintPointOnPerpBisector(Point &p, Line &l, int tagId, bool driving) { 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) +int System::addConstraintPointOnPerpBisector(Point &p, Point &lp1, Point &lp2, int tagId, bool driving) { 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) +int System::addConstraintParallel(Line &l1, Line &l2, int tagId, bool driving) { Constraint *constr = new ConstraintParallel(l1, l2); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPerpendicular(Line &l1, Line &l2, int tagId) +int System::addConstraintPerpendicular(Line &l1, Line &l2, int tagId, bool driving) { 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) + Point &l2p1, Point &l2p2, int tagId, bool driving) { 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) +int System::addConstraintL2LAngle(Line &l1, Line &l2, double *angle, int tagId, bool driving) { 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) + Point &l2p1, Point &l2p2, double *angle, int tagId, bool driving) { 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) +int System::addConstraintAngleViaPoint(Curve &crv1, Curve &crv2, Point &p, double *angle, int tagId, bool driving) { 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) +int System::addConstraintMidpointOnLine(Line &l1, Line &l2, int tagId, bool driving) { 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) + Point &l2p1, Point &l2p2, int tagId, bool driving) { 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, - bool internal, int tagId) + bool internal, int tagId, bool driving) { Constraint *constr = new ConstraintTangentCircumf(p1, p2, rad1, rad2, internal); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } // derived constraints -int System::addConstraintP2PCoincident(Point &p1, Point &p2, int tagId) +int System::addConstraintP2PCoincident(Point &p1, Point &p2, int tagId, bool driving) { - addConstraintEqual(p1.x, p2.x, tagId); - return addConstraintEqual(p1.y, p2.y, tagId); + addConstraintEqual(p1.x, p2.x, tagId, driving); + return addConstraintEqual(p1.y, p2.y, tagId, driving); } -int System::addConstraintHorizontal(Line &l, int tagId) +int System::addConstraintHorizontal(Line &l, int tagId, bool driving) { - return addConstraintEqual(l.p1.y, l.p2.y, tagId); + return addConstraintEqual(l.p1.y, l.p2.y, tagId, driving); } -int System::addConstraintHorizontal(Point &p1, Point &p2, int tagId) +int System::addConstraintHorizontal(Point &p1, Point &p2, int tagId, bool driving) { - return addConstraintEqual(p1.y, p2.y, tagId); + return addConstraintEqual(p1.y, p2.y, tagId, driving); } -int System::addConstraintVertical(Line &l, int tagId) +int System::addConstraintVertical(Line &l, int tagId, bool driving) { - return addConstraintEqual(l.p1.x, l.p2.x, tagId); + return addConstraintEqual(l.p1.x, l.p2.x, tagId, driving); } -int System::addConstraintVertical(Point &p1, Point &p2, int tagId) +int System::addConstraintVertical(Point &p1, Point &p2, int tagId, bool driving) { - return addConstraintEqual(p1.x, p2.x, tagId); + return addConstraintEqual(p1.x, p2.x, tagId, driving); } -int System::addConstraintCoordinateX(Point &p, double *x, int tagId) +int System::addConstraintCoordinateX(Point &p, double *x, int tagId, bool driving) { - return addConstraintEqual(p.x, x, tagId); + return addConstraintEqual(p.x, x, tagId, driving); } -int System::addConstraintCoordinateY(Point &p, double *y, int tagId) +int System::addConstraintCoordinateY(Point &p, double *y, int tagId, bool driving) { - return addConstraintEqual(p.y, y, tagId); + return addConstraintEqual(p.y, y, tagId, driving); } -int System::addConstraintArcRules(Arc &a, int tagId) +int System::addConstraintArcRules(Arc &a, int tagId, bool driving) { - addConstraintCurveValue(a.start, a, a.startAngle, tagId); - return addConstraintCurveValue(a.end, a, a.endAngle, tagId); + 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) +int System::addConstraintPointOnCircle(Point &p, Circle &c, int tagId, bool driving) { - return addConstraintP2PDistance(p, c.center, c.rad, tagId); + return addConstraintP2PDistance(p, c.center, c.rad, tagId, driving); } -int System::addConstraintPointOnEllipse(Point &p, Ellipse &e, int tagId) +int System::addConstraintPointOnEllipse(Point &p, Ellipse &e, int tagId, bool driving) { Constraint *constr = new ConstraintPointOnEllipse(p, e); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintPointOnHyperbolicArc(Point &p, ArcOfHyperbola &e, int tagId) +int System::addConstraintPointOnHyperbolicArc(Point &p, ArcOfHyperbola &e, int tagId, bool driving) { Constraint *constr = new ConstraintPointOnHyperbola(p, e); constr->setTag(tagId); - return addConstraint(constr); -} - -int System::addConstraintPointOnParabolicArc(Point &p, ArcOfParabola &e, int tagId) -{ - Constraint *constr = new ConstraintPointOnParabola(p, e); - constr->setTag(tagId); - return addConstraint(constr); -} - -int System::addConstraintArcOfEllipseRules(ArcOfEllipse &a, int tagId) -{ - addConstraintCurveValue(a.start,a,a.startAngle, tagId); - return addConstraintCurveValue(a.end,a,a.endAngle, tagId); -} - -int System::addConstraintCurveValue(Point &p, Curve &a, double *u, int tagId) -{ - Constraint *constr = new ConstraintCurveValue(p,p.x,a,u); - constr->setTag(tagId); - addConstraint(constr); - constr = new ConstraintCurveValue(p,p.y,a,u); - constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintArcOfHyperbolaRules(ArcOfHyperbola &a, int tagId) +int System::addConstraintPointOnParabolicArc(Point &p, ArcOfParabola &e, int tagId, bool driving) { - addConstraintCurveValue(a.start,a,a.startAngle, tagId); - return addConstraintCurveValue(a.end,a,a.endAngle, tagId); + Constraint *constr = new ConstraintPointOnParabola(p, e); + constr->setTag(tagId); + constr->setDriving(driving); + return addConstraint(constr); } -int System::addConstraintArcOfParabolaRules(ArcOfParabola &a, int tagId) +int System::addConstraintArcOfEllipseRules(ArcOfEllipse &a, int tagId, bool driving) { - addConstraintCurveValue(a.start,a,a.startAngle, tagId); - return addConstraintCurveValue(a.end,a,a.endAngle, tagId); + 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) +int System::addConstraintCurveValue(Point &p, Curve &a, double *u, int tagId, bool driving) { - return addConstraintP2PDistance(p, a.center, a.rad, tagId); + 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->setTag(tagId); + constr->setDriving(driving); + return addConstraint(constr); +} + +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); +} + +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); +} + +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) + int tagId, bool driving) { - addConstraintP2PCoincident(p2, a.start, tagId); + addConstraintP2PCoincident(p2, a.start, tagId, driving); double dx = *(p2.x) - *(p1.x); double dy = *(p2.y) - *(p1.y); if (dx * cos(*(a.startAngle)) + dy * sin(*(a.startAngle)) > 0) - return addConstraintP2PAngle(p1, p2, a.startAngle, 0, tagId); + return addConstraintP2PAngle(p1, p2, a.startAngle, 0, tagId, driving); else - return addConstraintP2PAngle(p1, p2, a.startAngle, M_PI, tagId); + return addConstraintP2PAngle(p1, p2, a.startAngle, M_PI, tagId, driving); } int System::addConstraintPerpendicularArc2Line(Arc &a, Point &p1, Point &p2, - int tagId) + int tagId, bool driving) { - addConstraintP2PCoincident(p1, a.end, tagId); + addConstraintP2PCoincident(p1, a.end, tagId, driving); double dx = *(p2.x) - *(p1.x); double dy = *(p2.y) - *(p1.y); if (dx * cos(*(a.endAngle)) + dy * sin(*(a.endAngle)) > 0) - return addConstraintP2PAngle(p1, p2, a.endAngle, 0, tagId); + return addConstraintP2PAngle(p1, p2, a.endAngle, 0, tagId, driving); else - return addConstraintP2PAngle(p1, p2, a.endAngle, M_PI, tagId); + return addConstraintP2PAngle(p1, p2, a.endAngle, M_PI, tagId, driving); } int System::addConstraintPerpendicularCircle2Arc(Point ¢er, double *radius, - Arc &a, int tagId) + Arc &a, int tagId, bool driving) { - addConstraintP2PDistance(a.start, center, radius, tagId); + addConstraintP2PDistance(a.start, center, radius, tagId, driving); 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); if (dx * cos(tangAngle) + dy * sin(tangAngle) > 0) - return addConstraintP2PAngle(center, a.start, a.startAngle, incrAngle, tagId); + return addConstraintP2PAngle(center, a.start, a.startAngle, incrAngle, tagId, driving); else - return addConstraintP2PAngle(center, a.start, a.startAngle, -incrAngle, tagId); + return addConstraintP2PAngle(center, a.start, a.startAngle, -incrAngle, tagId, driving); } int System::addConstraintPerpendicularArc2Circle(Arc &a, Point ¢er, - double *radius, int tagId) + double *radius, int tagId, bool driving) { - addConstraintP2PDistance(a.end, center, radius, tagId); + addConstraintP2PDistance(a.end, center, radius, tagId, driving); 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); if (dx * cos(tangAngle) + dy * sin(tangAngle) > 0) - return addConstraintP2PAngle(center, a.end, a.endAngle, incrAngle, tagId); + return addConstraintP2PAngle(center, a.end, a.endAngle, incrAngle, tagId, driving); else - return addConstraintP2PAngle(center, a.end, a.endAngle, -incrAngle, tagId); + return addConstraintP2PAngle(center, a.end, a.endAngle, -incrAngle, tagId, driving); } int System::addConstraintPerpendicularArc2Arc(Arc &a1, bool reverse1, - Arc &a2, bool reverse2, int tagId) + Arc &a2, bool reverse2, int tagId, bool driving) { Point &p1 = reverse1 ? a1.start : a1.end; Point &p2 = reverse2 ? a2.end : a2.start; - addConstraintP2PCoincident(p1, p2, tagId); - return addConstraintPerpendicular(a1.center, p1, a2.center, p2, tagId); + addConstraintP2PCoincident(p1, p2, tagId, driving); + return addConstraintPerpendicular(a1.center, p1, a2.center, p2, tagId, driving); } -int System::addConstraintTangent(Line &l, Circle &c, int tagId) +int System::addConstraintTangent(Line &l, Circle &c, int tagId, bool driving) { - return addConstraintP2LDistance(c.center, l, c.rad, tagId); + return addConstraintP2LDistance(c.center, l, c.rad, tagId, driving); } -int System::addConstraintTangent(Line &l, Ellipse &e, int tagId) +int System::addConstraintTangent(Line &l, Ellipse &e, int tagId, bool driving) { Constraint *constr = new ConstraintEllipseTangentLine(l, e); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintTangent(Line &l, Arc &a, int tagId) +int System::addConstraintTangent(Line &l, Arc &a, int tagId, bool driving) { - return addConstraintP2LDistance(a.center, l, a.rad, tagId); + return addConstraintP2LDistance(a.center, l, a.rad, tagId, driving); } -int System::addConstraintTangent(Circle &c1, Circle &c2, int tagId) +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); + (d < *c1.rad || d < *c2.rad), tagId, driving); } -int System::addConstraintTangent(Arc &a1, Arc &a2, int tagId) +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); + (d < *a1.rad || d < *a2.rad), tagId, driving); } -int System::addConstraintTangent(Circle &c, Arc &a, int tagId) +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); + (d < *c.rad || d < *a.rad), tagId, driving); } -int System::addConstraintCircleRadius(Circle &c, double *radius, int tagId) +int System::addConstraintCircleRadius(Circle &c, double *radius, int tagId, bool driving) { - return addConstraintEqual(c.rad, radius, tagId); + return addConstraintEqual(c.rad, radius, tagId, driving); } -int System::addConstraintArcRadius(Arc &a, double *radius, int tagId) +int System::addConstraintArcRadius(Arc &a, double *radius, int tagId, bool driving) { - return addConstraintEqual(a.rad, radius, tagId); + return addConstraintEqual(a.rad, radius, tagId, driving); } -int System::addConstraintEqualLength(Line &l1, Line &l2, double *length, int tagId) +int System::addConstraintEqualLength(Line &l1, Line &l2, double *length, int tagId, bool driving) { - addConstraintP2PDistance(l1.p1, l1.p2, length, tagId); - return addConstraintP2PDistance(l2.p1, l2.p2, length, tagId); + addConstraintP2PDistance(l1.p1, l1.p2, length, tagId, driving); + return addConstraintP2PDistance(l2.p1, l2.p2, length, tagId, driving); } -int System::addConstraintEqualRadius(Circle &c1, Circle &c2, int tagId) +int System::addConstraintEqualRadius(Circle &c1, Circle &c2, int tagId, bool driving) { - return addConstraintEqual(c1.rad, c2.rad, tagId); + return addConstraintEqual(c1.rad, c2.rad, tagId, driving); } -int System::addConstraintEqualRadii(Ellipse &e1, Ellipse &e2, int tagId) +int System::addConstraintEqualRadii(Ellipse &e1, Ellipse &e2, int tagId, bool driving) { - addConstraintEqual(e1.radmin, e2.radmin, tagId); + addConstraintEqual(e1.radmin, e2.radmin, tagId, driving); Constraint *constr = new ConstraintEqualMajorAxesConic(&e1,&e2); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintEqualRadii(ArcOfHyperbola &a1, ArcOfHyperbola &a2, int tagId) +int System::addConstraintEqualRadii(ArcOfHyperbola &a1, ArcOfHyperbola &a2, int tagId, bool driving) { - addConstraintEqual(a1.radmin, a2.radmin, tagId); + addConstraintEqual(a1.radmin, a2.radmin, tagId, driving); Constraint *constr = new ConstraintEqualMajorAxesConic(&a1,&a2); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintEqualFocus(ArcOfParabola &a1, ArcOfParabola &a2, int tagId) +int System::addConstraintEqualFocus(ArcOfParabola &a1, ArcOfParabola &a2, int tagId, bool driving) { Constraint *constr = new ConstraintEqualFocalDistance(&a1,&a2); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintEqualRadius(Circle &c1, Arc &a2, int tagId) +int System::addConstraintEqualRadius(Circle &c1, Arc &a2, int tagId, bool driving) { - return addConstraintEqual(c1.rad, a2.rad, tagId); + return addConstraintEqual(c1.rad, a2.rad, tagId, driving); } -int System::addConstraintEqualRadius(Arc &a1, Arc &a2, int tagId) +int System::addConstraintEqualRadius(Arc &a1, Arc &a2, int tagId, bool driving) { - return addConstraintEqual(a1.rad, a2.rad, tagId); + return addConstraintEqual(a1.rad, a2.rad, tagId, driving); } -int System::addConstraintP2PSymmetric(Point &p1, Point &p2, Line &l, int tagId) +int System::addConstraintP2PSymmetric(Point &p1, Point &p2, Line &l, int tagId, bool driving) { - addConstraintPerpendicular(p1, p2, l.p1, l.p2, tagId); - return addConstraintMidpointOnLine(p1, p2, l.p1, l.p2, tagId); + 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) +int System::addConstraintP2PSymmetric(Point &p1, Point &p2, Point &p, int tagId, bool driving) { - addConstraintPointOnPerpBisector(p, p1, p2, tagId); - return addConstraintPointOnLine(p, p1, p2, tagId); + 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) + int tagId, bool driving) { Constraint *constr = new ConstraintSnell(ray1,ray2,boundary,p,n1,n2,flipn1,flipn2); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintInternalAlignmentPoint2Ellipse(Ellipse &e, Point &p1, InternalAlignmentType alignmentType, int tagId) +int System::addConstraintInternalAlignmentPoint2Ellipse(Ellipse &e, Point &p1, InternalAlignmentType alignmentType, int tagId, bool driving) { Constraint *constr = new ConstraintInternalAlignmentPoint2Ellipse(e, p1, alignmentType); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintInternalAlignmentPoint2Hyperbola(Hyperbola &e, Point &p1, InternalAlignmentType alignmentType, int tagId) +int System::addConstraintInternalAlignmentPoint2Hyperbola(Hyperbola &e, Point &p1, InternalAlignmentType alignmentType, int tagId, bool driving) { Constraint *constr = new ConstraintInternalAlignmentPoint2Hyperbola(e, p1, alignmentType); constr->setTag(tagId); + constr->setDriving(driving); return addConstraint(constr); } -int System::addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse &e, Point &p1, Point &p2, int tagId) +int System::addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse &e, Point &p1, Point &p2, int tagId, bool driving) { double X_1=*p1.x; double Y_1=*p1.y; @@ -904,21 +992,21 @@ int System::addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse &e, Point if(closertopositivemajor>0){ //p2 is closer to positivemajor. Assign constraints back-to-front. - addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMajorX,tagId); - addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMajorY,tagId); - addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMajorX,tagId); - return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMajorY,tagId); + addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMajorX,tagId, driving); + addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMajorY,tagId, driving); + addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMajorX,tagId, driving); + return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMajorY,tagId, driving); } else{ //p1 is closer to positivemajor - addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMajorX,tagId); - addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMajorY,tagId); - addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMajorX,tagId); - return addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMajorY,tagId); + addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMajorX,tagId, driving); + addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMajorY,tagId, driving); + addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMajorX,tagId, driving); + return addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMajorY,tagId, driving); } } -int System::addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse &e, Point &p1, Point &p2, int tagId) +int System::addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse &e, Point &p1, Point &p2, int tagId, bool driving) { double X_1=*p1.x; double Y_1=*p1.y; @@ -939,31 +1027,31 @@ int System::addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse &e, Point + b*(X_F1 - X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2); if(closertopositiveminor>0){ - addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMinorX,tagId); - addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMinorY,tagId); - addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMinorX,tagId); - return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMinorY,tagId); + addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMinorX,tagId, driving); + addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMinorY,tagId, driving); + addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMinorX,tagId, driving); + return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMinorY,tagId, driving); } else { - addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMinorX,tagId); - addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMinorY,tagId); - addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMinorX,tagId); - return addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMinorY,tagId); + addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMinorX,tagId, driving); + addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMinorY,tagId, driving); + addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMinorX,tagId, driving); + return addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMinorY,tagId, driving); } } -int System::addConstraintInternalAlignmentEllipseFocus1(Ellipse &e, Point &p1, int tagId) +int System::addConstraintInternalAlignmentEllipseFocus1(Ellipse &e, Point &p1, int tagId, bool driving) { - addConstraintEqual(e.focus1.x, p1.x, tagId); - return addConstraintEqual(e.focus1.y, p1.y, tagId); + addConstraintEqual(e.focus1.x, p1.x, tagId, driving); + return addConstraintEqual(e.focus1.y, p1.y, tagId, driving); } -int System::addConstraintInternalAlignmentEllipseFocus2(Ellipse &e, Point &p1, int tagId) +int System::addConstraintInternalAlignmentEllipseFocus2(Ellipse &e, Point &p1, int tagId, bool driving) { - addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseFocus2X,tagId); - return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseFocus2Y,tagId); + addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseFocus2X,tagId, driving); + return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseFocus2Y,tagId, driving); } -int System::addConstraintInternalAlignmentHyperbolaMajorDiameter(Hyperbola &e, Point &p1, Point &p2, int tagId) +int System::addConstraintInternalAlignmentHyperbolaMajorDiameter(Hyperbola &e, Point &p1, Point &p2, int tagId, bool driving) { double X_1=*p1.x; double Y_1=*p1.y; @@ -994,21 +1082,21 @@ int System::addConstraintInternalAlignmentHyperbolaMajorDiameter(Hyperbola &e, P if(closertopositivemajor>0){ //p2 is closer to positivemajor. Assign constraints back-to-front. - addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaPositiveMajorX,tagId); - addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaPositiveMajorY,tagId); - addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaNegativeMajorX,tagId); - return addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaNegativeMajorY,tagId); + addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaPositiveMajorX,tagId, driving); + addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaPositiveMajorY,tagId, driving); + addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaNegativeMajorX,tagId, driving); + return addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaNegativeMajorY,tagId, driving); } else{ //p1 is closer to positivemajor - addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaPositiveMajorX,tagId); - addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaPositiveMajorY,tagId); - addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaNegativeMajorX,tagId); - return addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaNegativeMajorY,tagId); + addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaPositiveMajorX,tagId, driving); + addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaPositiveMajorY,tagId, driving); + addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaNegativeMajorX,tagId, driving); + return addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaNegativeMajorY,tagId, driving); } } -int System::addConstraintInternalAlignmentHyperbolaMinorDiameter(Hyperbola &e, Point &p1, Point &p2, int tagId) +int System::addConstraintInternalAlignmentHyperbolaMinorDiameter(Hyperbola &e, Point &p1, Point &p2, int tagId, bool driving) { double X_1=*p1.x; double Y_1=*p1.y; @@ -1036,35 +1124,35 @@ int System::addConstraintInternalAlignmentHyperbolaMinorDiameter(Hyperbola &e, P 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2); if(closertopositiveminor<0){ - addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaPositiveMinorX,tagId); - addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaPositiveMinorY,tagId); - addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaNegativeMinorX,tagId); - return addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaNegativeMinorY,tagId); + addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaPositiveMinorX,tagId, driving); + addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaPositiveMinorY,tagId, driving); + addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaNegativeMinorX,tagId, driving); + return addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaNegativeMinorY,tagId, driving); } else { - addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaPositiveMinorX,tagId); - addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaPositiveMinorY,tagId); - addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaNegativeMinorX,tagId); - return addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaNegativeMinorY,tagId); + addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaPositiveMinorX,tagId, driving); + addConstraintInternalAlignmentPoint2Hyperbola(e,p1,HyperbolaPositiveMinorY,tagId, driving); + addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaNegativeMinorX,tagId, driving); + return addConstraintInternalAlignmentPoint2Hyperbola(e,p2,HyperbolaNegativeMinorY,tagId, driving); } } -int System::addConstraintInternalAlignmentHyperbolaFocus(Hyperbola &e, Point &p1, int tagId) +int System::addConstraintInternalAlignmentHyperbolaFocus(Hyperbola &e, Point &p1, int tagId, bool driving) { - addConstraintEqual(e.focus1.x, p1.x, tagId); - return addConstraintEqual(e.focus1.y, p1.y, tagId); + addConstraintEqual(e.focus1.x, p1.x, tagId, driving); + return addConstraintEqual(e.focus1.y, p1.y, tagId, driving); } -int System::addConstraintInternalAlignmentParabolaFocus(Parabola &e, Point &p1, int tagId) +int System::addConstraintInternalAlignmentParabolaFocus(Parabola &e, Point &p1, int tagId, bool driving) { - addConstraintEqual(e.focus1.x, p1.x, tagId); - return addConstraintEqual(e.focus1.y, p1.y, tagId); + addConstraintEqual(e.focus1.x, p1.x, tagId, driving); + return addConstraintEqual(e.focus1.y, p1.y, tagId, driving); } -int System::addConstraintInternalAlignmentBSplineControlPoint(BSpline &b, Circle &c, int poleindex, int tagId) +int System::addConstraintInternalAlignmentBSplineControlPoint(BSpline &b, Circle &c, int poleindex, int tagId, bool driving) { - addConstraintEqual(b.poles[poleindex].x, c.center.x, tagId); - addConstraintEqual(b.poles[poleindex].y, c.center.y, tagId); - return addConstraintEqual(b.weights[poleindex], c.rad, tagId); + addConstraintEqual(b.poles[poleindex].x, c.center.x, tagId, driving); + addConstraintEqual(b.poles[poleindex].y, c.center.y, tagId, driving); + return addConstraintEqual(b.weights[poleindex], c.rad, tagId, driving); } //calculates angle between two curves at point of their intersection p. If two @@ -1134,6 +1222,12 @@ void System::declareUnknowns(VEC_pD ¶ms) hasUnknowns = true; } +void System::declareDrivenParams(VEC_pD ¶ms) +{ + pdrivenlist = params; +} + + void System::initSolution(Algorithm alg) { // - Stores the current parameters values in the vector "reference" @@ -3573,23 +3667,34 @@ int System::diagnose(Algorithm alg) // When adding an external geometry or a constraint on an external geometry the array 'plist' is empty. // So, we must abort here because otherwise we would create an invalid matrix and make the application // eventually crash. This fixes issues #0002372/#0002373. - if (plist.empty()) { + if (plist.empty() || (plist.size() - pdrivenlist.size()) == 0) { hasDiagnosis = true; - dofs = plist.size(); + dofs = 0; return dofs; } redundant.clear(); conflictingTags.clear(); redundantTags.clear(); - Eigen::MatrixXd J(clist.size(), plist.size()); + + // construct specific parameter list for diagonose ignoring driven constraint parameters + GCS::VEC_pD pdiagnoselist; + 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)) + pdiagnoselist.push_back(plist[j]); + } + + Eigen::MatrixXd J(clist.size(), pdiagnoselist.size()); int count=0; for (std::vector::iterator constr=clist.begin(); constr != clist.end(); ++constr) { (*constr)->revertParams(); - if ((*constr)->getTag() >= 0) { + if ((*constr)->getTag() >= 0 && (*constr)->isDriving()) { count++; - for (int j=0; j < int(plist.size()); j++) - J(count-1,j) = (*constr)->grad(plist[j]); + for (int j=0; j < int(pdiagnoselist.size()); j++) { + J(count-1,j) = (*constr)->grad(pdiagnoselist[j]); + } } } @@ -3613,22 +3718,24 @@ int System::diagnose(Algorithm alg) #endif #ifdef _GCS_DEBUG - // Debug code starts - std::stringstream stream; - - stream << '\n' << "J=" << '\n'; - stream << "["; - stream << J ; - stream << "]" << '\n'; +#ifdef _DEBUG_TO_FILE + std::ofstream stream; + stream.open("GCS_debug.txt", std::ofstream::out | std::ofstream::app); + stream << "GCS::System::diagnose()" << std::endl; + stream.flush(); + stream.close(); +#endif - const std::string tmp = stream.str(); - - Base::Console().Log(tmp.c_str()); - // Debug code ends + LogMatrix("J",J); #endif Eigen::MatrixXd R; - // Eigen::MatrixXd Q; // Obtaining the Q matrix with Sparse QR is buggy, see comments below + +#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX + Eigen::MatrixXd Q; // Obtaining the Q matrix with Sparse QR is buggy, see comments below + Eigen::MatrixXd R2; // Intended for a trapezoidal matrix, where R is the top triangular matrix of the R2 trapezoidal matrix +#endif + int paramsNum = 0; int constrNum = 0; int rank = 0; @@ -3650,7 +3757,10 @@ int System::diagnose(Algorithm alg) R = qrJT.matrixQR().topRows(constrNum) .triangularView(); - //Q = qrJT.matrixQ(); +#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX + R2 = qrJT.matrixQR(); + Q = qrJT.matrixQ(); +#endif } } #ifdef EIGEN_SPARSEQR_COMPATIBLE @@ -3660,26 +3770,32 @@ int System::diagnose(Algorithm alg) // Do not ask for Q Matrix!! // At Eigen 3.2 still has a bug that this only works for square matrices // if enabled it will crash - //Eigen::SparseMatrix Q = qrJT.matrixQ(); - //qrJT.matrixQ().evalTo(Q); - + #ifdef SPARSE_Q_MATRIX + Q = SqrJT.matrixQ(); + //Q = QS; + #endif + paramsNum = SqrJT.rows(); constrNum = SqrJT.cols(); SqrJT.setPivotThreshold(qrpivotThreshold); rank = SqrJT.rank(); - + if (constrNum >= paramsNum) R = SqrJT.matrixR().triangularView(); else R = SqrJT.matrixR().topRows(constrNum) - .triangularView(); - + .triangularView(); + + #ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX + R2 = SqrJT.matrixR(); + #endif } } #endif if(debugMode==IterationLevel) { std::stringstream stream; + stream << (qrAlgorithm==EigenSparseQR?"EigenSparseQR":(qrAlgorithm==EigenDenseQR?"DenseQR":"")); if (J.rows() > 0) { @@ -3707,19 +3823,36 @@ int System::diagnose(Algorithm alg) } const std::string tmp = stream.str(); - Base::Console().Log(tmp.c_str()); + + LogString(tmp); + } if (J.rows() > 0) { #ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX LogMatrix("R", R); - //LogMatrix("Q", Q); + LogMatrix("R2", R2); - LogMatrix("RowTransp", qrJT.rowsTranspositions()); + if(qrAlgorithm == EigenDenseQR){ // There is no rowsTranspositions in SparseQR. obtaining Q is buggy in Eigen for SparseQR + LogMatrix("Q", Q); + LogMatrix("RowTransp", qrJT.rowsTranspositions()); + } +#ifdef SPARSE_Q_MATRIX + else if(qrAlgorithm == EigenSparseQR) { + LogMatrix("Q", Q); + } +#endif #endif - // Detecting constraint solver parameters + // DETECTING CONSTRAINT SOLVER PARAMETERS + // + // NOTE: This is only true for dense QR with full pivoting, because solve parameters get reordered. + // I am unable to adapt it to Sparse QR. (abdullah). See: + // + // https://stackoverflow.com/questions/49009771/getting-rows-transpositions-with-sparse-qr + // https://forum.kde.org/viewtopic.php?f=74&t=151239 + // // R (original version, not R here which is trimmed to not have empty rows) // has paramsNum rows, the first "rank" rows correspond to parameters that are constraint @@ -3728,21 +3861,26 @@ int System::diagnose(Algorithm alg) rowPermutations.setIdentity(paramsNum); - const MatrixIndexType rowTranspositions = qrJT.rowsTranspositions(); + if(qrAlgorithm==EigenDenseQR){ // P.J.P' = Q.R see https://eigen.tuxfamily.org/dox/classEigen_1_1FullPivHouseholderQR.html + const MatrixIndexType rowTranspositions = qrJT.rowsTranspositions(); -/* if(qrAlgorithm==EigenDenseQR){ - rowTranspositions = qrJT.rowsTranspositions(); + for(int k = 0; k < rank; ++k) + rowPermutations.applyTranspositionOnTheRight(k, rowTranspositions.coeff(k)); } #ifdef EIGEN_SPARSEQR_COMPATIBLE - else if(qrAlgorithm==EigenSparseQR){ - //rowTranspositions = SqrJT.rowsTranspositions(); + else if(qrAlgorithm==EigenSparseQR){ + // J.P = Q.R, see https://eigen.tuxfamily.org/dox/classEigen_1_1SparseQR.html + // There is no rowsTransposition in this QR decomposition. + // TODO: This detection method won't work for SparseQR } -#endif*/ - for(int k = 0; k < rank; ++k) - rowPermutations.applyTranspositionOnTheRight(k, rowTranspositions.coeff(k)); +#endif - // params (in the order of J) shown as independent from QR +#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX + std::stringstream stream; +#endif + +// params (in the order of J) shown as independent from QR std::set indepParamCols; std::set depParamCols; @@ -3755,10 +3893,16 @@ int System::diagnose(Algorithm alg) // NOTE: Q*R = transpose(J), so the row of R corresponds to the col of J (the rows of transpose(J)). // The cols of J are the parameters, the rows are the constraints. #ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX - Base::Console().Log("R row %d = J col %d\n",j,origRow); + stream << "R row " << j << " = J col " << origRow << std::endl; #endif } +#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX + std::string tmp = stream.str(); + + LogString(tmp); +#endif + // If not independent, must be dependent for(int j=0; j < paramsNum; j++) { auto result = std::find(indepParamCols.begin(), indepParamCols.end(), j); @@ -3791,18 +3935,23 @@ int System::diagnose(Algorithm alg) } }*/ #ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX - Base::Console().Log("Indep params: ["); - for(auto indep :indepParamCols) - Base::Console().Log("%d,",indep); - Base::Console().Log("]\n"); + stream.flush(); - Base::Console().Log("Dep params: ["); + stream << "Indep params: ["; + for(auto indep :indepParamCols) + stream << indep; + stream << "]" << std::endl; + + stream << "Dep params: ["; for(auto dep :depParamCols) - Base::Console().Log("%d,",dep); - Base::Console().Log("]\n"); + stream << dep; + stream << "]" << std::endl; + + tmp = stream.str(); + LogString(tmp); #endif for( auto param : depParamCols) { - pdependentparameters.push_back(plist[param]); + pdependentparameters.push_back(pdiagnoselist[param]); } // Detecting conflicting or redundant constraints @@ -3893,7 +4042,7 @@ int System::diagnose(Algorithm alg) clistTmp.push_back(*constr); } - SubSystem *subSysTmp = new SubSystem(clistTmp, plist); + SubSystem *subSysTmp = new SubSystem(clistTmp, pdiagnoselist); int res = solve(subSysTmp,true,alg,true); if(debugMode==Minimal || debugMode==IterationLevel) { @@ -3985,7 +4134,7 @@ int System::diagnose(Algorithm alg) } hasDiagnosis = true; - dofs = plist.size(); + dofs = pdiagnoselist.size(); return dofs; } diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index bbde79fcf6..17d57c8c44 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -75,6 +75,7 @@ namespace GCS // 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; VEC_pD pdependentparameters; // list of dependent parameters by the system @@ -145,97 +146,97 @@ namespace GCS void removeConstraint(Constraint *constr); // basic constraints - int addConstraintEqual(double *param1, double *param2, int tagId=0); + int addConstraintEqual(double *param1, double *param2, int tagId=0, bool driving = true); int addConstraintDifference(double *param1, double *param2, - double *difference, int tagId=0); - int addConstraintP2PDistance(Point &p1, Point &p2, double *distance, int tagId=0); + 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); - int addConstraintP2PAngle(Point &p1, Point &p2, double *angle, int tagId=0); - int addConstraintP2LDistance(Point &p, Line &l, double *distance, int tagId=0); - int addConstraintPointOnLine(Point &p, Line &l, int tagId=0); - int addConstraintPointOnLine(Point &p, Point &lp1, Point &lp2, int tagId=0); - int addConstraintPointOnPerpBisector(Point &p, Line &l, int tagId=0); - int addConstraintPointOnPerpBisector(Point &p, Point &lp1, Point &lp2, int tagId=0); - int addConstraintParallel(Line &l1, Line &l2, int tagId=0); - int addConstraintPerpendicular(Line &l1, Line &l2, int tagId=0); + 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); - int addConstraintL2LAngle(Line &l1, Line &l2, double *angle, int tagId=0); + 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); + double *angle, int tagId=0, bool driving = true); int addConstraintAngleViaPoint(Curve &crv1, Curve &crv2, Point &p, - double *angle, int tagId=0); - int addConstraintMidpointOnLine(Line &l1, Line &l2, int tagId=0); + 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); + int tagId=0, bool driving = true); int addConstraintTangentCircumf(Point &p1, Point &p2, double *rd1, double *rd2, - bool internal=false, int tagId=0); + bool internal=false, int tagId=0, bool driving = true); // derived constraints - int addConstraintP2PCoincident(Point &p1, Point &p2, int tagId=0); - int addConstraintHorizontal(Line &l, int tagId=0); - int addConstraintHorizontal(Point &p1, Point &p2, int tagId=0); - int addConstraintVertical(Line &l, int tagId=0); - int addConstraintVertical(Point &p1, Point &p2, int tagId=0); - int addConstraintCoordinateX(Point &p, double *x, int tagId=0); - int addConstraintCoordinateY(Point &p, double *y, int tagId=0); - int addConstraintArcRules(Arc &a, int tagId=0); - int addConstraintPointOnCircle(Point &p, Circle &c, int tagId=0); - int addConstraintPointOnEllipse(Point &p, Ellipse &e, int tagId=0); - int addConstraintPointOnHyperbolicArc(Point &p, ArcOfHyperbola &e, int tagId=0); - int addConstraintPointOnParabolicArc(Point &p, ArcOfParabola &e, int tagId=0); - int addConstraintArcOfEllipseRules(ArcOfEllipse &a, int tagId=0); - int addConstraintCurveValue(Point &p, Curve &a, double *u, int tagId=0); - int addConstraintArcOfHyperbolaRules(ArcOfHyperbola &a, int tagId=0); - int addConstraintArcOfParabolaRules(ArcOfParabola &a, int tagId=0); - int addConstraintPointOnArc(Point &p, Arc &a, int tagId=0); + 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 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); + int tagId=0, bool driving = true); int addConstraintPerpendicularArc2Line(Arc &a, Point &p1, Point &p2, - int tagId=0); + int tagId=0, bool driving = true); int addConstraintPerpendicularCircle2Arc(Point ¢er, double *radius, Arc &a, - int tagId=0); + int tagId=0, bool driving = true); int addConstraintPerpendicularArc2Circle(Arc &a, Point ¢er, double *radius, - int tagId=0); + int tagId=0, bool driving = true); int addConstraintPerpendicularArc2Arc(Arc &a1, bool reverse1, - Arc &a2, bool reverse2, int tagId=0); - int addConstraintTangent(Line &l, Circle &c, int tagId=0); - int addConstraintTangent(Line &l, Ellipse &e, int tagId=0); - int addConstraintTangent(Line &l, Arc &a, int tagId=0); - int addConstraintTangent(Circle &c1, Circle &c2, int tagId=0); - int addConstraintTangent(Arc &a1, Arc &a2, int tagId=0); - int addConstraintTangent(Circle &c, Arc &a, int tagId=0); + 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); - int addConstraintArcRadius(Arc &a, double *radius, int tagId=0); - int addConstraintEqualLength(Line &l1, Line &l2, double *length, int tagId=0); - int addConstraintEqualRadius(Circle &c1, Circle &c2, int tagId=0); - int addConstraintEqualRadii(Ellipse &e1, Ellipse &e2, int tagId=0); - int addConstraintEqualRadii(ArcOfHyperbola &a1, ArcOfHyperbola &a2, int tagId=0); - int addConstraintEqualRadius(Circle &c1, Arc &a2, int tagId=0); - int addConstraintEqualRadius(Arc &a1, Arc &a2, int tagId=0); - int addConstraintEqualFocus(ArcOfParabola &a1, ArcOfParabola &a2, int tagId=0); - int addConstraintP2PSymmetric(Point &p1, Point &p2, Line &l, int tagId=0); - int addConstraintP2PSymmetric(Point &p1, Point &p2, Point &p, int tagId=0); + 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 addConstraintEqualLength(Line &l1, Line &l2, double *length, 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 tagId, bool driving = true); // internal alignment constraints - int addConstraintInternalAlignmentPoint2Ellipse(Ellipse &e, Point &p1, InternalAlignmentType alignmentType, int tagId=0); - int addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse &e, Point &p1, Point &p2, int tagId=0); - int addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse &e, Point &p1, Point &p2, int tagId=0); - int addConstraintInternalAlignmentEllipseFocus1(Ellipse &e, Point &p1, int tagId=0); - int addConstraintInternalAlignmentEllipseFocus2(Ellipse &e, Point &p1, int tagId=0); - int addConstraintInternalAlignmentPoint2Hyperbola(Hyperbola &e, Point &p1, InternalAlignmentType alignmentType, int tagId=0); - int addConstraintInternalAlignmentHyperbolaMajorDiameter(Hyperbola &e, Point &p1, Point &p2, int tagId=0); - int addConstraintInternalAlignmentHyperbolaMinorDiameter(Hyperbola &e, Point &p1, Point &p2, int tagId=0); - int addConstraintInternalAlignmentHyperbolaFocus(Hyperbola &e, Point &p1, int tagId=0); - int addConstraintInternalAlignmentParabolaFocus(Parabola &e, Point &p1, int tagId=0); - int addConstraintInternalAlignmentBSplineControlPoint(BSpline &b, Circle &c, int poleindex, int tag=0); + 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, int poleindex, int tag=0, bool driving = true); double calculateAngleViaPoint(Curve &crv1, Curve &crv2, Point &p); double calculateAngleViaPoint(Curve &crv1, Curve &crv2, Point &p1, Point &p2); @@ -251,6 +252,7 @@ namespace GCS void rescaleConstraint(int id, double coeff); void declareUnknowns(VEC_pD ¶ms); + void declareDrivenParams(VEC_pD ¶ms); void initSolution(Algorithm alg=DogLeg); int solve(bool isFine=true, Algorithm alg=DogLeg, bool isRedundantsolving=false);