diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index 7f282bbfc..9a842d79f 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -33,7 +33,7 @@ #include "qp_eq.h" // NOTE: In CMakeList.txt -DEIGEN_NO_DEBUG is set (it does not work with a define here), to solve this: -// this is needed to fix this SparseQR crash http://forum.freecadweb.org/viewtopic.php?f=10&t=11341&p=92146#p92146, +// this is needed to fix this SparseQR crash http://forum.freecadweb.org/viewtopic.php?f=10&t=11341&p=92146#p92146, // until Eigen library fixes its own problem with the assertion (definitely not solved in 3.2.0 branch) // NOTE2: solved in eigen3.3 @@ -41,8 +41,8 @@ + EIGEN_MAJOR_VERSION * 100 \ + EIGEN_MINOR_VERSION) -#if EIGEN_VERSION >= 30202 -#if EIGEN_VERSION < 30290 // this is eigen3.3. Bad numbering? This should be safe anyway. +#if EIGEN_VERSION >= 30202 +#if EIGEN_VERSION < 30290 // this is eigen3.3. Bad numbering? This should be safe anyway #define EIGEN_SPARSEQR_COMPATIBLE #endif #endif @@ -56,7 +56,7 @@ #include #endif -#undef _GCS_DEBUG +#undef _GCS_DEBUG #undef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ // this is to be enabled in Constraints.h when needed. @@ -216,9 +216,9 @@ System::System() { // currently Eigen only supports multithreading for multiplications // There is no appreciable gain from using more threads - #ifdef EIGEN_SPARSEQR_COMPATIBLE +#ifdef EIGEN_SPARSEQR_COMPATIBLE Eigen::setNbThreads(1); - #endif +#endif } /*DeepSOIC: seriously outdated, needs redesign @@ -579,11 +579,11 @@ int System::addConstraintEllipticalArcRangeToEndPoints(Point &p, ArcOfEllipse &a int System::addConstraintArcOfEllipseRules(ArcOfEllipse &a, int tagId) -{ - addConstraintEllipticalArcRangeToEndPoints(a.start,a,a.startAngle, tagId); - addConstraintEllipticalArcRangeToEndPoints(a.end,a,a.endAngle, tagId); - - addConstraintPointOnEllipse(a.start, a, tagId); +{ + addConstraintEllipticalArcRangeToEndPoints(a.start,a,a.startAngle, tagId); + addConstraintEllipticalArcRangeToEndPoints(a.end,a,a.endAngle, tagId); + + addConstraintPointOnEllipse(a.start, a, tagId); return addConstraintPointOnEllipse(a.end, a, tagId); } @@ -720,10 +720,9 @@ int System::addConstraintEqualRadius(Circle &c1, Circle &c2, int tagId) int System::addConstraintEqualRadii(Ellipse &e1, Ellipse &e2, int tagId) { - //addConstraintEqual(e1.radmaj, e2.radmaj, tagId); addConstraintEqual(e1.radmin, e2.radmin, tagId); - + Constraint *constr = new ConstraintEqualMajorAxesEllipse(e1,e2); constr->setTag(tagId); return addConstraint(constr); @@ -766,11 +765,11 @@ int System::addConstraintInternalAlignmentPoint2Ellipse(Ellipse &e, Point &p1, I { Constraint *constr = new ConstraintInternalAlignmentPoint2Ellipse(e, p1, alignmentType); constr->setTag(tagId); - return addConstraint(constr); + return addConstraint(constr); } int System::addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse &e, Point &p1, Point &p2, int tagId) -{ +{ double X_1=*p1.x; double Y_1=*p1.y; double X_2=*p2.x; @@ -780,7 +779,7 @@ int System::addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse &e, Point double X_F1=*e.focus1.x; double Y_F1=*e.focus1.y; double b=*e.radmin; - + // P1=vector([X_1,Y_1]) // P2=vector([X_2,Y_2]) // dF1= (F1-C)/sqrt((F1-C)*(F1-C)) @@ -797,13 +796,13 @@ int System::addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse &e, Point pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) - pow(Y_2 - Y_c - (Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2); - + 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); + return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMajorY,tagId); } else{ //p1 is closer to positivemajor @@ -825,7 +824,7 @@ int System::addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse &e, Point double X_F1=*e.focus1.x; double Y_F1=*e.focus1.y; double b=*e.radmin; - + // Same idea as for major above, but for minor // DMC=(P1-PA)*(P1-PA)-(P2-PA)*(P2-PA) double closertopositiveminor= pow(X_1 - X_c + b*(Y_F1 - Y_c)/sqrt(pow(X_F1 - X_c, 2) + @@ -833,12 +832,12 @@ int System::addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse &e, Point X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) + pow(-Y_1 + Y_c + b*(X_F1 - X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) - pow(-Y_2 + Y_c + 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); + return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMinorY,tagId); } else { addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMinorX,tagId); addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMinorY,tagId); @@ -865,7 +864,10 @@ int System::addConstraintInternalAlignmentEllipseFocus2(Ellipse &e, Point &p1, i //remote angle computation (this is useful when the endpoints haven't) been //made coincident yet double System::calculateAngleViaPoint(Curve &crv1, Curve &crv2, Point &p) - {return calculateAngleViaPoint(crv1, crv2, p, p);} +{ + return calculateAngleViaPoint(crv1, crv2, p, p); +} + double System::calculateAngleViaPoint(Curve &crv1, Curve &crv2, Point &p1, Point &p2) { GCS::DeriVector2 n1 = crv1.CalculateNormal(p1); @@ -942,7 +944,7 @@ void System::initSolution(Algorithm alg) // storing reference configuration setReference(); - + // diagnose conflicting or redundant constraints if (!hasDiagnosis) { diagnose(alg); @@ -951,10 +953,10 @@ void System::initSolution(Algorithm alg) } std::vector clistR; if (redundant.size()) { - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr) + for (std::vector::const_iterator constr=clist.begin(); constr != clist.end(); ++constr) { if (redundant.count(*constr) == 0) clistR.push_back(*constr); + } } else clistR = clist; @@ -998,9 +1000,10 @@ void System::initSolution(Algorithm alg) reducedConstrs.insert(*constr); double *p_kept = reducedParams[it1->second]; double *p_replaced = reducedParams[it2->second]; - for (int i=0; i < int(plist.size()); ++i) + for (int i=0; i < int(plist.size()); ++i) { if (reducedParams[i] == p_replaced) reducedParams[i] = p_kept; + } } } } @@ -1167,16 +1170,14 @@ int System::solve_BFGS(SubSystem *subsys, bool isFine, bool isRedundantsolving) if(debugMode==IterationLevel) { std::stringstream stream; - stream << "BFGS: convergence: " << (isRedundantsolving?convergenceRedundant:convergence) - << ", xsize: " << xsize + << ", xsize: " << xsize << ", maxIter: " << maxIterNumber << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); } - double divergingLim = 1e6*err + 1e12; double h_norm; @@ -1185,22 +1186,20 @@ int System::solve_BFGS(SubSystem *subsys, bool isFine, bool isRedundantsolving) if (h_norm <= (isRedundantsolving?convergenceRedundant:convergence) || err <= smallF){ if(debugMode==IterationLevel) { std::stringstream stream; - stream << "BFGS Converged!!: " - << ", err: " << err + << ", 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) { std::stringstream stream; - stream << "BFGS Failed: Diverging!!: " - << ", err: " << err + << ", err: " << err << ", divergingLim: " << divergingLim << "\n"; const std::string tmp = stream.str(); @@ -1233,14 +1232,13 @@ int System::solve_BFGS(SubSystem *subsys, bool isFine, bool isRedundantsolving) h = x; subsys->getParams(x); h = x - h; // = x - xold - + if(debugMode==IterationLevel) { std::stringstream stream; - stream << "BFGS, Iteration: " << iter << ", err: " << err << ", h_norm: " << h_norm << "\n"; - + const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); } @@ -1257,9 +1255,9 @@ int System::solve_BFGS(SubSystem *subsys, bool isFine, bool isRedundantsolving) int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) { - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ extractSubsystem(subsys, isRedundantsolving); - #endif +#endif int xsize = subsys->pSize(); int csize = subsys->cSize(); @@ -1281,27 +1279,26 @@ int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) int maxIterNumber = (isRedundantsolving? (sketchSizeMultiplierRedundant?maxIterRedundant * xsize:maxIterRedundant): (sketchSizeMultiplier?maxIter * xsize:maxIter)); - + 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); - + if(debugMode==IterationLevel) { std::stringstream stream; - stream << "LM: eps: " << eps << ", eps1: " << eps1 << ", tau: " << tau << ", convergence: " << (isRedundantsolving?convergenceRedundant:convergence) - << ", xsize: " << xsize + << ", 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) { @@ -1403,16 +1400,15 @@ int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) stop = 7; break; } - + 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"; - + const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); } @@ -1429,27 +1425,26 @@ int System::solve_LM(SubSystem* subsys, bool isRedundantsolving) int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) { - #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ extractSubsystem(subsys, isRedundantsolving); - #endif +#endif 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(); if (xsize == 0) return Success; - + int maxIterNumber = (isRedundantsolving? (sketchSizeMultiplierRedundant?maxIterRedundant * xsize:maxIterRedundant): (sketchSizeMultiplier?maxIter * xsize:maxIter)); - + if(debugMode==IterationLevel) { std::stringstream stream; - stream << "DL: tolg: " << tolg << ", tolx: " << tolx << ", tolf: " << tolf @@ -1461,7 +1456,7 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); - } + } Eigen::VectorXd x(xsize), x_new(xsize); Eigen::VectorXd fx(csize), fx_new(csize); @@ -1480,7 +1475,7 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) // 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 delta=0.1; @@ -1520,7 +1515,7 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) h_gn = Jx.adjoint()*(Jx*Jx.adjoint()).ldlt().solve(-fx); break; } - + double rel_error = (Jx*h_gn + fx).norm() / fx.norm(); if (rel_error > 1e15) break; @@ -1604,35 +1599,33 @@ int System::solve_DL(SubSystem* subsys, bool isRedundantsolving) } else reduce--; - + 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"; - + const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); } - + // count this iteration and start again iter++; } subsys->revertParams(); - + if(debugMode==IterationLevel) { std::stringstream stream; - stream << "DL: stopcode: " << stop << ((stop == 1) ? ", Success" : ", Failed") << "\n"; const std::string tmp = stream.str(); Base::Console().Log(tmp.c_str()); - } + } return (stop == 1) ? Success : Failed; } @@ -1670,7 +1663,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) for( VEC_pD::iterator it = plistout.begin(); it != plistout.end(); ++it, ++ips) { VEC_pD::iterator p = std::find(plist.begin(), plist.end(),(*it)); size_t p_index = std::distance(plist.begin(), p); - + if(p_index == plist.size()) { subsystemfile << "// Error: Subsystem parameter not in system params" << "address: " << (void *)(*it) << std::endl; } @@ -1680,21 +1673,21 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) int ic=0; // constraint index int icp=0; // index of constraint params not within SYSTEM params - for( std::vector::iterator it = clist_.begin(); it != clist_.end(); ++it,++ic) { + for (std::vector::iterator it = clist_.begin(); it != clist_.end(); ++it,++ic) { switch((*it)->getTypeId()) { - case Equal: + case Equal: { // 2 VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(),(*it)->pvec[0]); VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(),(*it)->pvec[1]); size_t i1 = std::distance(plist.begin(), p1); size_t i2 = std::distance(plist.begin(), p2); - + bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); size_t ni1 = std::distance(clist_params_.begin(), np1); - + if(i1 == plist.size()) { if(ni1 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1731,7 +1724,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i1 = std::distance(plist.begin(), p1); size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); - + bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); size_t ni1 = std::distance(clist_params_.begin(), np1); @@ -1763,7 +1756,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb3=false; VEC_pD::iterator np3 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[2]); size_t ni3 = std::distance(clist_params_.begin(), np3); - + if(i3 == plist.size()) { if(ni3 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1776,7 +1769,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) subsystemfile << "clist_.push_back(new ConstraintDifference(" << (npb1?("clist_params_["):("plist_[")) << (npb1?ni1:i1) << "]," \ << (npb2?("clist_params_["):("plist_[")) << (npb2?ni2:i2) << "]," \ - << (npb3?("clist_params_["):("plist_[")) << (npb3?ni3:i3) << "])); // addresses = "<< (*it)->pvec[0] << "," << (*it)->pvec[1] << "," << (*it)->pvec[2] << std::endl; + << (npb3?("clist_params_["):("plist_[")) << (npb3?ni3:i3) << "])); // addresses = "<< (*it)->pvec[0] << "," << (*it)->pvec[1] << "," << (*it)->pvec[2] << std::endl; break; } case P2PDistance: @@ -1790,12 +1783,12 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); - size_t i5 = std::distance(plist.begin(), p5); - + size_t i5 = std::distance(plist.begin(), p5); + bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); size_t ni1 = std::distance(clist_params_.begin(), np1); - + if(i1 == plist.size()) { if(ni1 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1809,7 +1802,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb2=false; VEC_pD::iterator np2 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[1]); size_t ni2 = std::distance(clist_params_.begin(), np2); - + if(i2 == plist.size()) { if(ni2 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1823,7 +1816,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb3=false; VEC_pD::iterator np3 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[2]); size_t ni3 = std::distance(clist_params_.begin(), np3); - + if(i3 == plist.size()) { if(ni3 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1836,7 +1829,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb4=false; VEC_pD::iterator np4 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[3]); - size_t ni4 = std::distance(clist_params_.begin(), np4); + size_t ni4 = std::distance(clist_params_.begin(), np4); if(i4 == plist.size()) { if(ni4 == clist_params_.size()) { @@ -1851,7 +1844,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb5=false; VEC_pD::iterator np5 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[4]); size_t ni5 = std::distance(clist_params_.begin(), np5); - + if(i5 == plist.size()) { if(ni5 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1885,11 +1878,11 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); size_t i5 = std::distance(plist.begin(), p5); - + bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); size_t ni1 = std::distance(clist_params_.begin(), np1); - + if(i1 == plist.size()) { if(ni1 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1903,7 +1896,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb2=false; VEC_pD::iterator np2 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[1]); size_t ni2 = std::distance(clist_params_.begin(), np2); - + if(i2 == plist.size()) { if(ni2 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1917,7 +1910,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb3=false; VEC_pD::iterator np3 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[2]); size_t ni3 = std::distance(clist_params_.begin(), np3); - + if(i3 == plist.size()) { if(ni3 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1945,7 +1938,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb5=false; VEC_pD::iterator np5 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[4]); size_t ni5 = std::distance(clist_params_.begin(), np5); - + if(i5 == plist.size()) { if(ni5 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -1980,9 +1973,9 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); - size_t i5 = std::distance(plist.begin(), p5); + size_t i5 = std::distance(plist.begin(), p5); size_t i6 = std::distance(plist.begin(), p6); - size_t i7 = std::distance(plist.begin(), p7); + size_t i7 = std::distance(plist.begin(), p7); bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); @@ -2001,7 +1994,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb2=false; VEC_pD::iterator np2 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[1]); size_t ni2 = std::distance(clist_params_.begin(), np2); - + if(i2 == plist.size()) { if(ni2 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2029,7 +2022,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb4=false; VEC_pD::iterator np4 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[3]); size_t ni4 = std::distance(clist_params_.begin(), np4); - + if(i4 == plist.size()) { if(ni4 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2039,11 +2032,11 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) } npb4=true; } - + bool npb5=false; VEC_pD::iterator np5 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[4]); size_t ni5 = std::distance(clist_params_.begin(), np5); - + if(i5 == plist.size()) { if(ni5 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2057,7 +2050,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb6=false; VEC_pD::iterator np6 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[5]); size_t ni6 = std::distance(clist_params_.begin(), np6); - + if(i6 == plist.size()) { if(ni6 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2071,7 +2064,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb7=false; VEC_pD::iterator np7 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[6]); size_t ni7 = std::distance(clist_params_.begin(), np7); - + if(i7 == plist.size()) { if(ni7 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2089,7 +2082,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) subsystemfile << "c" << ic << "->pvec.push_back(" << (npb4?("clist_params_["):("plist_[")) << (npb4?ni4:i4) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb5?("clist_params_["):("plist_[")) << (npb5?ni5:i5) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb6?("clist_params_["):("plist_[")) << (npb6?ni6:i6) <<"]);" << std::endl; - subsystemfile << "c" << ic << "->pvec.push_back(" << (npb7?("clist_params_["):("plist_[")) << (npb7?ni7:i7) <<"]);" << std::endl; + subsystemfile << "c" << ic << "->pvec.push_back(" << (npb7?("clist_params_["):("plist_[")) << (npb7?ni7:i7) <<"]);" << std::endl; subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl; subsystemfile << "c" << ic << "->rescale();" << std::endl; subsystemfile << "clist_.push_back(c" << ic << "); // addresses = "<< (*it)->pvec[0] << "," << (*it)->pvec[1] << "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << "," << (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6] << std::endl; @@ -2107,8 +2100,8 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); - size_t i5 = std::distance(plist.begin(), p5); - size_t i6 = std::distance(plist.begin(), p6); + size_t i5 = std::distance(plist.begin(), p5); + size_t i6 = std::distance(plist.begin(), p6); bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); @@ -2127,7 +2120,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb2=false; VEC_pD::iterator np2 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[1]); size_t ni2 = std::distance(clist_params_.begin(), np2); - + if(i2 == plist.size()) { if(ni2 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2155,7 +2148,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb4=false; VEC_pD::iterator np4 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[3]); size_t ni4 = std::distance(clist_params_.begin(), np4); - + if(i4 == plist.size()) { if(ni4 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2169,7 +2162,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb5=false; VEC_pD::iterator np5 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[4]); size_t ni5 = std::distance(clist_params_.begin(), np5); - + if(i5 == plist.size()) { if(ni5 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2218,8 +2211,8 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); - size_t i5 = std::distance(plist.begin(), p5); - size_t i6 = std::distance(plist.begin(), p6); + size_t i5 = std::distance(plist.begin(), p5); + size_t i6 = std::distance(plist.begin(), p6); bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); @@ -2238,7 +2231,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb2=false; VEC_pD::iterator np2 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[1]); size_t ni2 = std::distance(clist_params_.begin(), np2); - + if(i2 == plist.size()) { if(ni2 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2252,7 +2245,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb3=false; VEC_pD::iterator np3 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[2]); size_t ni3 = std::distance(clist_params_.begin(), np3); - + if(i3 == plist.size()) { if(ni3 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2266,7 +2259,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb4=false; VEC_pD::iterator np4 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[3]); size_t ni4 = std::distance(clist_params_.begin(), np4); - + if(i4 == plist.size()) { if(ni4 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2331,15 +2324,15 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); - size_t i5 = std::distance(plist.begin(), p5); + size_t i5 = std::distance(plist.begin(), p5); size_t i6 = std::distance(plist.begin(), p6); size_t i7 = std::distance(plist.begin(), p7); size_t i8 = std::distance(plist.begin(), p8); - + bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); size_t ni1 = std::distance(clist_params_.begin(), np1); - + if(i1 == plist.size()) { if(ni1 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2367,7 +2360,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb3=false; VEC_pD::iterator np3 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[2]); size_t ni3 = std::distance(clist_params_.begin(), np3); - + if(i3 == plist.size()) { if(ni3 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2381,7 +2374,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb4=false; VEC_pD::iterator np4 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[3]); size_t ni4 = std::distance(clist_params_.begin(), np4); - + if(i4 == plist.size()) { if(ni4 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2395,7 +2388,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb5=false; VEC_pD::iterator np5 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[4]); size_t ni5 = std::distance(clist_params_.begin(), np5); - + if(i5 == plist.size()) { if(ni5 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2409,7 +2402,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb6=false; VEC_pD::iterator np6 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[5]); size_t ni6 = std::distance(clist_params_.begin(), np6); - + if(i6 == plist.size()) { if(ni6 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2423,7 +2416,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb7=false; VEC_pD::iterator np7 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[6]); size_t ni7 = std::distance(clist_params_.begin(), np7); - + if(i7 == plist.size()) { if(ni7 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2437,7 +2430,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb8=false; VEC_pD::iterator np8 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[7]); size_t ni8 = std::distance(clist_params_.begin(), np8); - + if(i8 == plist.size()) { if(ni8 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2456,12 +2449,12 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) subsystemfile << "c" << ic << "->pvec.push_back(" << (npb5?("clist_params_["):("plist_[")) << (npb5?ni5:i5) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb6?("clist_params_["):("plist_[")) << (npb6?ni6:i6) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb7?("clist_params_["):("plist_[")) << (npb7?ni7:i7) <<"]);" << std::endl; - subsystemfile << "c" << ic << "->pvec.push_back(" << (npb8?("clist_params_["):("plist_[")) << (npb8?ni8:i8) <<"]);" << std::endl; + subsystemfile << "c" << ic << "->pvec.push_back(" << (npb8?("clist_params_["):("plist_[")) << (npb8?ni8:i8) <<"]);" << std::endl; subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl; subsystemfile << "c" << ic << "->rescale();" << std::endl; subsystemfile << "clist_.push_back(c" << ic << "); // addresses = "<< (*it)->pvec[0] << "," << (*it)->pvec[1] << "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << "," << (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6] << "," << (*it)->pvec[7] << std::endl; break; - } + } case Perpendicular: { // 8 VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(),(*it)->pvec[0]); @@ -2476,7 +2469,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); - size_t i5 = std::distance(plist.begin(), p5); + size_t i5 = std::distance(plist.begin(), p5); size_t i6 = std::distance(plist.begin(), p6); size_t i7 = std::distance(plist.begin(), p7); size_t i8 = std::distance(plist.begin(), p8); @@ -2484,7 +2477,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); size_t ni1 = std::distance(clist_params_.begin(), np1); - + if(i1 == plist.size()) { if(ni1 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2512,7 +2505,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb3=false; VEC_pD::iterator np3 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[2]); size_t ni3 = std::distance(clist_params_.begin(), np3); - + if(i3 == plist.size()) { if(ni3 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2582,7 +2575,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb8=false; VEC_pD::iterator np8 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[7]); size_t ni8 = std::distance(clist_params_.begin(), np8); - + if(i8 == plist.size()) { if(ni8 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2601,7 +2594,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) subsystemfile << "c" << ic << "->pvec.push_back(" << (npb5?("clist_params_["):("plist_[")) << (npb5?ni5:i5) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb6?("clist_params_["):("plist_[")) << (npb6?ni6:i6) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb7?("clist_params_["):("plist_[")) << (npb7?ni7:i7) <<"]);" << std::endl; - subsystemfile << "c" << ic << "->pvec.push_back(" << (npb8?("clist_params_["):("plist_[")) << (npb8?ni8:i8) <<"]);" << std::endl; + subsystemfile << "c" << ic << "->pvec.push_back(" << (npb8?("clist_params_["):("plist_[")) << (npb8?ni8:i8) <<"]);" << std::endl; subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl; subsystemfile << "c" << ic << "->rescale();" << std::endl; subsystemfile << "clist_.push_back(c" << ic << "); // addresses = "<< (*it)->pvec[0] << "," << (*it)->pvec[1] << "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << "," << (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6] << "," << (*it)->pvec[7] << std::endl; @@ -2622,16 +2615,16 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); - size_t i5 = std::distance(plist.begin(), p5); + size_t i5 = std::distance(plist.begin(), p5); size_t i6 = std::distance(plist.begin(), p6); size_t i7 = std::distance(plist.begin(), p7); size_t i8 = std::distance(plist.begin(), p8); size_t i9 = std::distance(plist.begin(), p9); - + bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); size_t ni1 = std::distance(clist_params_.begin(), np1); - + if(i1 == plist.size()) { if(ni1 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2659,7 +2652,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb3=false; VEC_pD::iterator np3 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[2]); size_t ni3 = std::distance(clist_params_.begin(), np3); - + if(i3 == plist.size()) { if(ni3 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2701,7 +2694,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb6=false; VEC_pD::iterator np6 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[5]); size_t ni6 = std::distance(clist_params_.begin(), np6); - + if(i6 == plist.size()) { if(ni6 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2763,7 +2756,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) subsystemfile << "c" << ic << "->pvec.push_back(" << (npb6?("clist_params_["):("plist_[")) << (npb6?ni6:i6) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb7?("clist_params_["):("plist_[")) << (npb7?ni7:i7) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb8?("clist_params_["):("plist_[")) << (npb8?ni8:i8) <<"]);" << std::endl; - subsystemfile << "c" << ic << "->pvec.push_back(" << (npb9?("clist_params_["):("plist_[")) << (npb9?ni9:i9) <<"]);" << std::endl; + subsystemfile << "c" << ic << "->pvec.push_back(" << (npb9?("clist_params_["):("plist_[")) << (npb9?ni9:i9) <<"]);" << std::endl; subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl; subsystemfile << "c" << ic << "->rescale();" << std::endl; subsystemfile << "clist_.push_back(c" << ic << "); // addresses = "<< (*it)->pvec[0] << "," << (*it)->pvec[1] << "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << "," << (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6] << "," << (*it)->pvec[7] << "," << (*it)->pvec[8] << std::endl; @@ -2783,7 +2776,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); - size_t i5 = std::distance(plist.begin(), p5); + size_t i5 = std::distance(plist.begin(), p5); size_t i6 = std::distance(plist.begin(), p6); size_t i7 = std::distance(plist.begin(), p7); size_t i8 = std::distance(plist.begin(), p8); @@ -2805,7 +2798,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb2=false; VEC_pD::iterator np2 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[1]); size_t ni2 = std::distance(clist_params_.begin(), np2); - + if(i2 == plist.size()) { if(ni2 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2861,7 +2854,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb6=false; VEC_pD::iterator np6 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[5]); size_t ni6 = std::distance(clist_params_.begin(), np6); - + if(i6 == plist.size()) { if(ni6 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2889,7 +2882,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb8=false; VEC_pD::iterator np8 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[7]); size_t ni8 = std::distance(clist_params_.begin(), np8); - + if(i8 == plist.size()) { if(ni8 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2908,7 +2901,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) subsystemfile << "c" << ic << "->pvec.push_back(" << (npb5?("clist_params_["):("plist_[")) << (npb5?ni5:i5) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb6?("clist_params_["):("plist_[")) << (npb6?ni6:i6) <<"]);" << std::endl; subsystemfile << "c" << ic << "->pvec.push_back(" << (npb7?("clist_params_["):("plist_[")) << (npb7?ni7:i7) <<"]);" << std::endl; - subsystemfile << "c" << ic << "->pvec.push_back(" << (npb8?("clist_params_["):("plist_[")) << (npb8?ni8:i8) <<"]);" << std::endl; + subsystemfile << "c" << ic << "->pvec.push_back(" << (npb8?("clist_params_["):("plist_[")) << (npb8?ni8:i8) <<"]);" << std::endl; subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl; subsystemfile << "c" << ic << "->rescale();" << std::endl; subsystemfile << "clist_.push_back(c" << ic << "); // addresses = "<< (*it)->pvec[0] << "," << (*it)->pvec[1] << "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << "," << (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6] << "," << (*it)->pvec[7] << std::endl; @@ -2926,9 +2919,9 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) size_t i2 = std::distance(plist.begin(), p2); size_t i3 = std::distance(plist.begin(), p3); size_t i4 = std::distance(plist.begin(), p4); - size_t i5 = std::distance(plist.begin(), p5); - size_t i6 = std::distance(plist.begin(), p6); - + size_t i5 = std::distance(plist.begin(), p5); + size_t i6 = std::distance(plist.begin(), p6); + bool npb1=false; VEC_pD::iterator np1 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[0]); size_t ni1 = std::distance(clist_params_.begin(), np1); @@ -2960,7 +2953,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb3=false; VEC_pD::iterator np3 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[2]); size_t ni3 = std::distance(clist_params_.begin(), np3); - + if(i3 == plist.size()) { if(ni3 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -2988,7 +2981,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb5=false; VEC_pD::iterator np5 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[4]); size_t ni5 = std::distance(clist_params_.begin(), np5); - + if(i5 == plist.size()) { if(ni5 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -3002,7 +2995,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb6=false; VEC_pD::iterator np6 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[5]); size_t ni6 = std::distance(clist_params_.begin(), np6); - + if(i6 == plist.size()) { if(ni6 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -3115,7 +3108,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb6=false; VEC_pD::iterator np6 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[5]); size_t ni6 = std::distance(clist_params_.begin(), np6); - + if(i6 == plist.size()) { if(ni6 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -3129,7 +3122,7 @@ void System::extractSubsystem(SubSystem *subsys, bool isRedundantsolving) bool npb7=false; VEC_pD::iterator np7 = std::find(clist_params_.begin(), clist_params_.end(),(*it)->pvec[6]); size_t ni7 = std::distance(clist_params_.begin(), np7); - + if(i7 == plist.size()) { if(ni7 == clist_params_.size()) { subsystemfile << "// Address not in System params...rebuilding into clist_params_" << std::endl; @@ -3219,7 +3212,7 @@ int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool isFine, bool isRe int maxIterNumber = (isRedundantsolving? (sketchSizeMultiplierRedundant?maxIterRedundant * xsize:maxIterRedundant): (sketchSizeMultiplier?maxIter * xsize:maxIter)); - + double divergingLim = 1e6*subsysA->error() + 1e12; double mu = 0; @@ -3383,8 +3376,7 @@ int System::diagnose(Algorithm alg) redundantTags.clear(); Eigen::MatrixXd J(clist.size(), plist.size()); int count=0; - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) { + for (std::vector::iterator constr=clist.begin(); constr != clist.end(); ++constr) { (*constr)->revertParams(); if ((*constr)->getTag() >= 0) { count++; @@ -3392,10 +3384,10 @@ int System::diagnose(Algorithm alg) J(count-1,j) = (*constr)->grad(plist[j]); } } - + #ifdef EIGEN_SPARSEQR_COMPATIBLE Eigen::SparseMatrix SJ; - + if(qrAlgorithm==EigenSparseQR){ // this creation is not optimized (done using triplets) // however the time this takes is negligible compared to the @@ -3403,40 +3395,40 @@ int System::diagnose(Algorithm alg) SJ = J.sparseView(); SJ.makeCompressed(); } - + Eigen::SparseQR, Eigen::COLAMDOrdering > SqrJT; #else - 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"); + 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 - - #ifdef _GCS_DEBUG + +#ifdef _GCS_DEBUG // Debug code starts std::stringstream stream; - + stream << "["; stream << J ; stream << "]"; - + const std::string tmp = stream.str(); - + Base::Console().Log(tmp.c_str()); // Debug code ends - #endif - +#endif + Eigen::MatrixXd R; int paramsNum = 0; int constrNum = 0; int rank = 0; Eigen::FullPivHouseholderQR qrJT; - + if(qrAlgorithm==EigenDenseQR){ if (J.rows() > 0) { qrJT=Eigen::FullPivHouseholderQR(J.topRows(count).transpose()); Eigen::MatrixXd Q = qrJT.matrixQ (); - + paramsNum = qrJT.rows(); constrNum = qrJT.cols(); qrJT.setThreshold(qrpivotThreshold); @@ -3446,80 +3438,79 @@ int System::diagnose(Algorithm alg) R = qrJT.matrixQR().triangularView(); else R = qrJT.matrixQR().topRows(constrNum) - .triangularView(); + .triangularView(); } } - #ifdef EIGEN_SPARSEQR_COMPATIBLE +#ifdef EIGEN_SPARSEQR_COMPATIBLE else if(qrAlgorithm==EigenSparseQR){ if (SJ.rows() > 0) { SqrJT=Eigen::SparseQR, Eigen::COLAMDOrdering >(SJ.topRows(count).transpose()); // 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(); + //Eigen::SparseMatrix Q = qrJT.matrixQ(); //qrJT.matrixQ().evalTo(Q); - + 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(); } } - #endif - +#endif + if(debugMode==IterationLevel) { std::stringstream stream; - stream << (qrAlgorithm==EigenSparseQR?"EigenSparseQR":(qrAlgorithm==EigenDenseQR?"DenseQR":"")); - + stream << (qrAlgorithm==EigenSparseQR?"EigenSparseQR":(qrAlgorithm==EigenDenseQR?"DenseQR":"")); + if (J.rows() > 0) { stream - #ifdef EIGEN_SPARSEQR_COMPATIBLE +#ifdef EIGEN_SPARSEQR_COMPATIBLE << ", Threads: " << Eigen::nbThreads() - #endif - #ifdef EIGEN_VECTORIZE +#endif +#ifdef EIGEN_VECTORIZE << ", Vectorization: On" - #endif - << ", Pivot Threshold: " << qrpivotThreshold +#endif + << ", Pivot Threshold: " << qrpivotThreshold << ", Params: " << paramsNum << ", Constr: " << constrNum << ", Rank: " << rank << "\n"; } else { - stream - #ifdef EIGEN_SPARSEQR_COMPATIBLE + stream +#ifdef EIGEN_SPARSEQR_COMPATIBLE << ", Threads: " << Eigen::nbThreads() - #endif - #ifdef EIGEN_VECTORIZE +#endif +#ifdef EIGEN_VECTORIZE << ", Vectorization: On" - #endif - << ", Empty Sketch, nothing to solve" << "\n"; +#endif + << ", Empty Sketch, nothing to solve" << "\n"; } - + const std::string tmp = stream.str(); - Base::Console().Log(tmp.c_str()); + Base::Console().Log(tmp.c_str()); } - + if (J.rows() > 0) { - - #ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX +#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX // Debug code starts std::stringstream stream; - + stream << "["; stream << R ; stream << "]"; - + const std::string tmp = stream.str(); - + Base::Console().Log(tmp.c_str()); // Debug code ends - #endif +#endif if (constrNum > rank) { // conflicting or redundant constraints for (int i=1; i < rank; i++) { @@ -3538,27 +3529,27 @@ int System::diagnose(Algorithm alg) for (int row=0; row < rank; row++) { if (fabs(R(row,j)) > 1e-10) { int origCol = 0; - + if(qrAlgorithm==EigenDenseQR) origCol=qrJT.colsPermutation().indices()[row]; - #ifdef EIGEN_SPARSEQR_COMPATIBLE +#ifdef EIGEN_SPARSEQR_COMPATIBLE else if(qrAlgorithm==EigenSparseQR) origCol=SqrJT.colsPermutation().indices()[row]; - #endif - +#endif + conflictGroups[j-rank].push_back(clist[origCol]); } } int origCol = 0; - + if(qrAlgorithm==EigenDenseQR) origCol=qrJT.colsPermutation().indices()[j]; - - #ifdef EIGEN_SPARSEQR_COMPATIBLE + +#ifdef EIGEN_SPARSEQR_COMPATIBLE else if(qrAlgorithm==EigenSparseQR) - origCol=SqrJT.colsPermutation().indices()[j]; - #endif - + origCol=SqrJT.colsPermutation().indices()[j]; +#endif + conflictGroups[j-rank].push_back(clist[origCol]); } @@ -3580,7 +3571,7 @@ int System::diagnose(Algorithm alg) } if (conflictingMap.empty()) break; - + int maxPopularity = 0; Constraint *mostPopular = NULL; for (std::map< Constraint *, SET_I >::const_iterator it=conflictingMap.begin(); @@ -3609,7 +3600,7 @@ int System::diagnose(Algorithm alg) SubSystem *subSysTmp = new SubSystem(clistTmp, plist); int res = solve(subSysTmp,true,alg,true); - + if(debugMode==Minimal || debugMode==IterationLevel) { std::string solvername; switch (alg) { @@ -3622,12 +3613,11 @@ int System::diagnose(Algorithm alg) 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(); @@ -3637,9 +3627,9 @@ int System::diagnose(Algorithm alg) redundant.insert(*constr); } resetToReference(); - - if(debugMode==Minimal || debugMode==IterationLevel) { - Base::Console().Log("Sketcher Redundant solving: %d redundants\n",redundant.size()); + + if(debugMode==Minimal || debugMode==IterationLevel) { + Base::Console().Log("Sketcher Redundant solving: %d redundants\n",redundant.size()); } std::vector< std::vector > conflictGroupsOrig=conflictGroups; @@ -3679,9 +3669,10 @@ int System::diagnose(Algorithm alg) redundantTagsSet.insert((*constr)->getTag()); // remove tags represented at least in one non-redundant constraint for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) + 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()); @@ -3697,6 +3688,7 @@ int System::diagnose(Algorithm alg) dofs = paramsNum - rank; return dofs; } + hasDiagnosis = true; dofs = plist.size(); return dofs; @@ -3788,8 +3780,9 @@ double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir) void free(VEC_pD &doublevec) { for (VEC_pD::iterator it = doublevec.begin(); - it != doublevec.end(); ++it) + it != doublevec.end(); ++it) { if (*it) delete *it; + } doublevec.clear(); } @@ -3841,8 +3834,9 @@ void free(std::vector &constrvec) void free(std::vector &subsysvec) { for (std::vector::iterator it=subsysvec.begin(); - it != subsysvec.end(); ++it) + it != subsysvec.end(); ++it) { if (*it) delete *it; + } }