diff --git a/src/system.cpp b/src/system.cpp index 89d315938..d475ab9e5 100644 --- a/src/system.cpp +++ b/src/system.cpp @@ -316,32 +316,62 @@ bool System::NewtonSolve() { for(i = 0; i < mat.m; i++) { mat.B.num[i] = (mat.B.sym[i])->Eval(); } + + // Where we were before we took the step, so that we can take a shorter + // one from the same operating point if the first one is no good. + std::vector prevVal(mat.n); + + // Take the Newton step, + // J(x_n) (x_{n+1} - x_n) = 0 - F(x_n) + // scaled by relax, and re-evaluate the functions, since the params have + // just changed. Returns false if that took us somewhere clearly useless. + auto takeStep = [&](double relax) { + for(int k = 0; k < mat.n; k++) { + Param *p = param.FindById(mat.param[k]); + p->val = prevVal[k] - relax*mat.X[k]; + if(IsReasonable(p->val)) return false; + } + for(int k = 0; k < mat.m; k++) { + mat.B.num[k] = (mat.B.sym[k])->Eval(); + if(IsReasonable(mat.B.num[k])) return false; + } + return true; + }; + do { // And evaluate the Jacobian at our initial operating point. EvalJacobian(); if(!SolveLeastSquares()) break; - // Take the Newton step; - // J(x_n) (x_{n+1} - x_n) = 0 - F(x_n) for(i = 0; i < mat.n; i++) { - Param *p = param.FindById(mat.param[i]); - p->val -= mat.X[i]; - if(IsReasonable(p->val)) { - // Very bad, and clearly not convergent - return false; - } + prevVal[i] = param.FindById(mat.param[i])->val; } - - // Re-evalute the functions, since the params have just changed. - for(i = 0; i < mat.m; i++) { - mat.B.num[i] = (mat.B.sym[i])->Eval(); - if(IsReasonable(mat.B.num[i])) { + const double err = mat.B.num.squaredNorm(); + + // A Newton step is only as good as the linearization of F about our + // operating point, so where F is strongly curved the full step can + // land much further from the solution than it started; and if it + // lands near a critical point of F, then the step after that one is + // enormous, and the geometry runs away to nowhere. But the step is a + // descent direction for |F|^2, so if the full step makes things + // worse, then a short enough step along the same direction makes + // them better. So backtrack until the residual actually decreases. + bool accepted = false; + double relax = 1.0; + for(int tries = 0; tries < 8 && !accepted; tries++, relax /= 2) { + accepted = takeStep(relax) && mat.B.num.squaredNorm() < err; + } + if(!accepted) { + // No step along this direction is an improvement, so just take + // the whole thing, like we always used to; if that was a bad + // idea, then the iteration limit below will catch it. + if(!takeStep(1.0)) { // Very bad, and clearly not convergent return false; } } - + // Check for convergence converged = true; for(i = 0; i < mat.m; i++) { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0dafd42db..e5357429f 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -19,6 +19,7 @@ set(testsuite_SOURCES core/expr/test.cpp core/locale/test.cpp core/path/test.cpp + core/solver/test.cpp constraint/points_coincident/test.cpp constraint/pt_pt_distance/test.cpp constraint/pt_plane_distance/test.cpp diff --git a/test/core/solver/angle.slvs b/test/core/solver/angle.slvs new file mode 100644 index 000000000..ba213cde5 --- /dev/null +++ b/test/core/solver/angle.slvs @@ -0,0 +1,456 @@ +±²³SolveSpaceREVa + + +Group.h.v=00000001 +Group.type=5000 +Group.name=#references +Group.color=ff000000 +Group.skipFirst=0 +Group.predef.swapUV=0 +Group.predef.negateU=0 +Group.predef.negateV=0 +Group.visible=1 +Group.suppress=0 +Group.relaxConstraints=0 +Group.allowRedundant=0 +Group.allDimsReference=0 +Group.remap={ +} +AddGroup + +Group.h.v=00000002 +Group.type=5001 +Group.order=1 +Group.name=sketch-in-plane +Group.activeWorkplane.v=80020000 +Group.color=ff000000 +Group.subtype=6000 +Group.skipFirst=0 +Group.predef.q.w=1.00000000000000000000 +Group.predef.origin.v=00010001 +Group.predef.swapUV=0 +Group.predef.negateU=0 +Group.predef.negateV=0 +Group.visible=1 +Group.suppress=0 +Group.relaxConstraints=0 +Group.allowRedundant=0 +Group.allDimsReference=0 +Group.remap={ +} +AddGroup + +Param.h.v.=00010010 +AddParam + +Param.h.v.=00010011 +AddParam + +Param.h.v.=00010012 +AddParam + +Param.h.v.=00010020 +Param.val=1.00000000000000000000 +AddParam + +Param.h.v.=00010021 +AddParam + +Param.h.v.=00010022 +AddParam + +Param.h.v.=00010023 +AddParam + +Param.h.v.=00020010 +AddParam + +Param.h.v.=00020011 +AddParam + +Param.h.v.=00020012 +AddParam + +Param.h.v.=00020020 +Param.val=0.50000000000000000000 +AddParam + +Param.h.v.=00020021 +Param.val=0.50000000000000000000 +AddParam + +Param.h.v.=00020022 +Param.val=0.50000000000000000000 +AddParam + +Param.h.v.=00020023 +Param.val=0.50000000000000000000 +AddParam + +Param.h.v.=00030010 +AddParam + +Param.h.v.=00030011 +AddParam + +Param.h.v.=00030012 +AddParam + +Param.h.v.=00030020 +Param.val=0.50000000000000000000 +AddParam + +Param.h.v.=00030021 +Param.val=-0.50000000000000000000 +AddParam + +Param.h.v.=00030022 +Param.val=-0.50000000000000000000 +AddParam + +Param.h.v.=00030023 +Param.val=-0.50000000000000000000 +AddParam + +Param.h.v.=00040010 +Param.val=-59.55307006835937500000 +AddParam + +Param.h.v.=00040011 +Param.val=-21.02559858317918184412 +AddParam + +Param.h.v.=00040013 +Param.val=40.44692993164062500000 +AddParam + +Param.h.v.=00040014 +Param.val=-21.02559858317918184412 +AddParam + +Param.h.v.=00050010 +Param.val=40.44692993164062500000 +AddParam + +Param.h.v.=00050011 +Param.val=-21.02559858317918184412 +AddParam + +Param.h.v.=00050013 +Param.val=40.44692993164062500000 +AddParam + +Param.h.v.=00050014 +Param.val=36.70942833578337882727 +AddParam + +Param.h.v.=00060010 +Param.val=40.44692993164062500000 +AddParam + +Param.h.v.=00060011 +Param.val=36.70942833578337882727 +AddParam + +Param.h.v.=00060013 +Param.val=-59.55307006835937500000 +AddParam + +Param.h.v.=00060014 +Param.val=-21.02559858317918184412 +AddParam + +Request.h.v=00000001 +Request.type=100 +Request.group.v=00000001 +Request.construction=0 +AddRequest + +Request.h.v=00000002 +Request.type=100 +Request.group.v=00000001 +Request.construction=0 +AddRequest + +Request.h.v=00000003 +Request.type=100 +Request.group.v=00000001 +Request.construction=0 +AddRequest + +Request.h.v=00000004 +Request.type=200 +Request.workplane.v=80020000 +Request.group.v=00000002 +Request.construction=0 +AddRequest + +Request.h.v=00000005 +Request.type=200 +Request.workplane.v=80020000 +Request.group.v=00000002 +Request.construction=0 +AddRequest + +Request.h.v=00000006 +Request.type=200 +Request.workplane.v=80020000 +Request.group.v=00000002 +Request.construction=0 +AddRequest + +Entity.h.v=00010000 +Entity.type=10000 +Entity.construction=0 +Entity.point[0].v=00010001 +Entity.normal.v=00010020 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00010001 +Entity.type=2000 +Entity.construction=1 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00010020 +Entity.type=3000 +Entity.construction=0 +Entity.point[0].v=00010001 +Entity.actNormal.w=1.00000000000000000000 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00020000 +Entity.type=10000 +Entity.construction=0 +Entity.point[0].v=00020001 +Entity.normal.v=00020020 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00020001 +Entity.type=2000 +Entity.construction=1 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00020020 +Entity.type=3000 +Entity.construction=0 +Entity.point[0].v=00020001 +Entity.actNormal.w=0.50000000000000000000 +Entity.actNormal.vx=0.50000000000000000000 +Entity.actNormal.vy=0.50000000000000000000 +Entity.actNormal.vz=0.50000000000000000000 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00030000 +Entity.type=10000 +Entity.construction=0 +Entity.point[0].v=00030001 +Entity.normal.v=00030020 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00030001 +Entity.type=2000 +Entity.construction=1 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00030020 +Entity.type=3000 +Entity.construction=0 +Entity.point[0].v=00030001 +Entity.actNormal.w=0.50000000000000000000 +Entity.actNormal.vx=-0.50000000000000000000 +Entity.actNormal.vy=-0.50000000000000000000 +Entity.actNormal.vz=-0.50000000000000000000 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00040000 +Entity.type=11000 +Entity.construction=0 +Entity.point[0].v=00040001 +Entity.point[1].v=00040002 +Entity.workplane.v=80020000 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00040001 +Entity.type=2001 +Entity.construction=0 +Entity.workplane.v=80020000 +Entity.actPoint.x=-59.55307006835937500000 +Entity.actPoint.y=-21.02559858317918184412 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00040002 +Entity.type=2001 +Entity.construction=0 +Entity.workplane.v=80020000 +Entity.actPoint.x=40.44692993164062500000 +Entity.actPoint.y=-21.02559858317918184412 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00050000 +Entity.type=11000 +Entity.construction=0 +Entity.point[0].v=00050001 +Entity.point[1].v=00050002 +Entity.workplane.v=80020000 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00050001 +Entity.type=2001 +Entity.construction=0 +Entity.workplane.v=80020000 +Entity.actPoint.x=40.44692993164062500000 +Entity.actPoint.y=-21.02559858317918184412 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00050002 +Entity.type=2001 +Entity.construction=0 +Entity.workplane.v=80020000 +Entity.actPoint.x=40.44692993164062500000 +Entity.actPoint.y=36.70942833578337882727 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00060000 +Entity.type=11000 +Entity.construction=0 +Entity.point[0].v=00060001 +Entity.point[1].v=00060002 +Entity.workplane.v=80020000 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00060001 +Entity.type=2001 +Entity.construction=0 +Entity.workplane.v=80020000 +Entity.actPoint.x=40.44692993164062500000 +Entity.actPoint.y=36.70942833578337882727 +Entity.actVisible=1 +AddEntity + +Entity.h.v=00060002 +Entity.type=2001 +Entity.construction=0 +Entity.workplane.v=80020000 +Entity.actPoint.x=-59.55307006835937500000 +Entity.actPoint.y=-21.02559858317918184412 +Entity.actVisible=1 +AddEntity + +Entity.h.v=80020000 +Entity.type=10000 +Entity.construction=0 +Entity.point[0].v=80020002 +Entity.normal.v=80020001 +Entity.actVisible=1 +AddEntity + +Entity.h.v=80020001 +Entity.type=3010 +Entity.construction=0 +Entity.point[0].v=80020002 +Entity.actNormal.w=1.00000000000000000000 +Entity.actVisible=1 +AddEntity + +Entity.h.v=80020002 +Entity.type=2012 +Entity.construction=1 +Entity.actVisible=1 +AddEntity + +Constraint.h.v=00000001 +Constraint.type=80 +Constraint.group.v=00000002 +Constraint.workplane.v=80020000 +Constraint.entityA.v=00040000 +Constraint.other=0 +Constraint.other2=0 +Constraint.reference=0 +AddConstraint + +Constraint.h.v=00000002 +Constraint.type=20 +Constraint.group.v=00000002 +Constraint.workplane.v=80020000 +Constraint.ptA.v=00040002 +Constraint.ptB.v=00050001 +Constraint.other=0 +Constraint.other2=0 +Constraint.reference=0 +AddConstraint + +Constraint.h.v=00000003 +Constraint.type=81 +Constraint.group.v=00000002 +Constraint.workplane.v=80020000 +Constraint.entityA.v=00050000 +Constraint.other=0 +Constraint.other2=0 +Constraint.reference=0 +AddConstraint + +Constraint.h.v=00000004 +Constraint.type=20 +Constraint.group.v=00000002 +Constraint.workplane.v=80020000 +Constraint.ptA.v=00050002 +Constraint.ptB.v=00060001 +Constraint.other=0 +Constraint.other2=0 +Constraint.reference=0 +AddConstraint + +Constraint.h.v=00000005 +Constraint.type=20 +Constraint.group.v=00000002 +Constraint.workplane.v=80020000 +Constraint.ptA.v=00040001 +Constraint.ptB.v=00060002 +Constraint.other=0 +Constraint.other2=0 +Constraint.reference=0 +AddConstraint + +Constraint.h.v=00000006 +Constraint.type=30 +Constraint.group.v=00000002 +Constraint.workplane.v=80020000 +Constraint.valA=100.00000000000000000000 +Constraint.ptA.v=00040001 +Constraint.ptB.v=00040002 +Constraint.other=0 +Constraint.other2=0 +Constraint.reference=0 +Constraint.disp.offset.y=-10.00000000000000000000 +AddConstraint + +Constraint.h.v=00000007 +Constraint.type=120 +Constraint.group.v=00000002 +Constraint.workplane.v=80020000 +Constraint.valA=30.00000000000000000000 +Constraint.entityA.v=00060000 +Constraint.entityB.v=00040000 +Constraint.other=1 +Constraint.other2=0 +Constraint.reference=0 +Constraint.disp.offset.x=-10.62718505859374573674 +Constraint.disp.offset.y=-14.77174072265624893419 +AddConstraint + diff --git a/test/core/solver/test.cpp b/test/core/solver/test.cpp new file mode 100644 index 000000000..1e8907246 --- /dev/null +++ b/test/core/solver/test.cpp @@ -0,0 +1,41 @@ +#include "solvespace.h" + +#include "harness.h" + +// angle.slvs is a right triangle: a horizontal line 100 mm long, a vertical +// line, and a third line closing them, with an angle constraint (handle 7) +// between that third line and the horizontal one. +static const hConstraint HANGLE = { 7 }; +// The endpoints of the vertical line, whose length is 100*tan(angle). +static const hEntity VERTICAL_A = { 0x00050001 }; +static const hEntity VERTICAL_B = { 0x00050002 }; + +// Change the value of a dimension and re-solve, the same way that the GUI +// does after the user edits one. +static SolveResult SetValueAndSolve(hConstraint hc, double value) { + Constraint *c = SK.GetConstraint(hc); + hGroup hg = c->group; + c->valA = value; + SS.MarkGroupDirty(hg); + SS.GenerateAll(); + return SK.GetGroup(hg)->solved.how; +} + +static double VerticalLength() { + return SK.GetEntity(VERTICAL_B)->PointGetNum().y - + SK.GetEntity(VERTICAL_A)->PointGetNum().y; +} + +TEST_CASE(angle_step_over_critical_point) { + CHECK_LOAD("angle.slvs"); + + // Both of these angles are perfectly solvable, but the undamped Newton + // step from the 60 degree solution towards the 30 degree one used to + // land right on top of a critical point of the angle equation, from + // where the next step threw the geometry off to infinity. + CHECK_TRUE(SetValueAndSolve(HANGLE, 60.0) == SolveResult::OKAY); + CHECK_EQ_EPS(VerticalLength(), 100*tan(60*PI/180)); + + CHECK_TRUE(SetValueAndSolve(HANGLE, 30.0) == SolveResult::OKAY); + CHECK_EQ_EPS(VerticalLength(), 100*tan(30*PI/180)); +} diff --git a/test/debugtool.cpp b/test/debugtool.cpp index aea6b8475..a1e984963 100644 --- a/test/debugtool.cpp +++ b/test/debugtool.cpp @@ -4,11 +4,95 @@ // Copyright 2017 whitequark //----------------------------------------------------------------------------- +#include "solvespace.h" #include "expr.h" #include "platform/platform.h" using namespace SolveSpace; +static const char *SolveResultName(SolveResult r) { + switch(r) { + case SolveResult::OKAY: return "OKAY"; + case SolveResult::DIDNT_CONVERGE: return "DIDNT_CONVERGE"; + case SolveResult::REDUNDANT_OKAY: return "REDUNDANT_OKAY"; + case SolveResult::REDUNDANT_DIDNT_CONVERGE: return "REDUNDANT_DIDNT_CONVERGE"; + case SolveResult::TOO_MANY_UNKNOWNS: return "TOO_MANY_UNKNOWNS"; + } + return "?"; +} + +struct StepResult { + SolveResult how; + double signedAngle; +}; + +// Set constraint `hc` to `value`, regenerate, and report how the group solved. +static StepResult SetAndSolve(uint32_t hcv, double value, bool verbose) { + hConstraint hc = {hcv}; + Constraint *c = SK.GetConstraint(hc); + hGroup hg = c->group; + c->valA = value; + SS.MarkGroupDirty(hg); + // This is what the GUI does after an edit: MarkGroupDirty() schedules a + // Generate::DIRTY pass, which is run from the event loop. + if(getenv("SS_GENERATE_ALL")) { + SS.GenerateAll(SolveSpaceUI::Generate::ALL); + } else { + SS.GenerateAll(); + } + + Group *g = SK.GetGroup(hg); + StepResult sr = { g->solved.how, 0.0 }; + + // Report the geometry too, not just the result code: converging to a + // mirrored or otherwise different solution is not a success. + c = SK.constraint.FindByIdNoOops(hc); + if(c && c->type == Constraint::Type::ANGLE) { + Vector a = SK.GetEntity(c->entityA)->VectorGetNum(); + Vector b = SK.GetEntity(c->entityB)->VectorGetNum(); + if(c->other) a = a.ScaledBy(-1); + a = a.ProjectVectorInto(c->workplane); + b = b.ProjectVectorInto(c->workplane); + Vector n = SK.GetEntity(c->workplane)->Normal()->NormalN(); + double dot = a.Dot(b) / (a.Magnitude() * b.Magnitude()); + double crs = a.Cross(b).Dot(n) / (a.Magnitude() * b.Magnitude()); + sr.signedAngle = atan2(crs, dot) * 180 / PI; + } + + if(verbose) { + fprintf(stderr, " set c%u = %.10g -> %s (dof=%d, bad=%d, signed angle=%.9g)\n", + hcv, value, SolveResultName(sr.how), g->solved.dof, g->solved.remove.n, + sr.signedAngle); + fprintf(stderr, " requests=%d constraints=%d entities=%d params=%d\n", + SK.request.n, SK.constraint.n, SK.entity.n, SK.param.n); + for(int i = 0; i < g->solved.remove.n; i++) { + Constraint *bc = SK.constraint.FindByIdNoOops(g->solved.remove[i]); + fprintf(stderr, " bad constraint %u type %d\n", g->solved.remove[i].v, + bc ? (int)bc->type : -1); + } + for(auto &e : SK.entity) { + if(e.group != hg) continue; + if(e.type != Entity::Type::POINT_IN_2D && e.type != Entity::Type::POINT_IN_3D) + continue; + Vector p = e.PointGetNum(); + fprintf(stderr, " pt %08x (%.6f, %.6f, %.6f)\n", e.h.v, p.x, p.y, p.z); + } + } + return sr; +} + +static bool LoadSketch(const std::string &file) { + SS.Init(); + SS.showToolbar = false; + SS.checkClosedContour = false; + if(!SS.LoadFromFile(Platform::Path::From(file))) { + fprintf(stderr, "cannot load %s\n", file.c_str()); + return false; + } + SS.AfterNewFile(); + return true; +} + int main(int argc, char **argv) { std::vector args = Platform::InitCli(argc, argv); @@ -21,6 +105,32 @@ int main(int argc, char **argv) { fprintf(stderr, "%g\n", e->Eval()); } Platform::FreeAllTemporary(); + } else if(args.size() >= 5 && args[1] == "solve") { + // solve [ ...] + if(!LoadSketch(args[2])) return 1; + uint32_t hcv = (uint32_t)strtoul(args[3].c_str(), NULL, 0); + for(size_t i = 4; i < args.size(); i++) { + SetAndSolve(hcv, strtod(args[i].c_str(), NULL), /*verbose=*/true); + } + Platform::FreeAllTemporary(); + } else if(args.size() == 8 && args[1] == "sweep") { + // sweep + uint32_t hcv = (uint32_t)strtoul(args[3].c_str(), NULL, 0); + double start = strtod(args[4].c_str(), NULL); + double from = strtod(args[5].c_str(), NULL); + double to = strtod(args[6].c_str(), NULL); + double step = strtod(args[7].c_str(), NULL); + for(double target = from; target <= to + step/2; target += step) { + if(!LoadSketch(args[2])) return 1; + StepResult a = SetAndSolve(hcv, start, /*verbose=*/false); + StepResult b = SetAndSolve(hcv, target, /*verbose=*/false); + printf("%.6g %.6g %s %s %.9g\n", start, target, + SolveResultName(a.how), SolveResultName(b.how), b.signedAngle); + fflush(stdout); + SK.Clear(); + SS.Clear(); + } + Platform::FreeAllTemporary(); } else { fprintf(stderr, "Usage: %s \n", args[0].c_str()); //-----------------------------------------------------------------------------> 80 col */ @@ -28,6 +138,12 @@ int main(int argc, char **argv) { Commands: expr [expr] Evaluate an expression. + solve [file.slvs] [constraint-handle] [value]... + Load a sketch, then repeatedly set a constraint's value and re-solve, + reporting the solve result of each step. + sweep [file.slvs] [constraint-handle] [start] [from] [to] [step] + For each target value in [from, to], load the sketch, solve it at + [start], then at the target, and report both solve results. )"); }