Solvespace

1
//-----------------------------------------------------------------------------
2
// Once we've written our constraint equations in the symbolic algebra system,
3
// these routines linearize them, and solve by a modified Newton's method.
4
// This also contains the routines to detect non-convergence or inconsistency,
5
// and report diagnostics to the user.
6
//
7
// Copyright 2008-2013 Jonathan Westhues.
8
//-----------------------------------------------------------------------------
9
#include "solvespace.h"
10

11
#include <Eigen/Core>
12
#include <Eigen/SparseQR>
13

14
// The solver will converge all unknowns to within this tolerance. This must
15
// always be much less than LENGTH_EPS, and in practice should be much less.
16
const double System::CONVERGE_TOLERANCE = (LENGTH_EPS/(1e2));
17

18
constexpr size_t LikelyPartialCountPerEq = 10;
19

20
bool System::WriteJacobian(int tag) {
21
    // Clear all
22
    mat.param.clear();
23
    mat.eq.clear();
24
    mat.A.sym.setZero();
25
    mat.B.sym.clear();
26

27
    for(Param &p : param) {
28
        if(p.tag != tag) continue;
29
        mat.param.push_back(p.h);
30
    }
31
    mat.n = mat.param.size();
32

33
    for(Equation &e : eq) {
34
        if(e.tag != tag) continue;
35
        mat.eq.push_back(&e);
36
    }
37
    mat.m = mat.eq.size();
38
    mat.A.sym.resize(mat.m, mat.n);
39
    mat.A.sym.reserve(Eigen::VectorXi::Constant(mat.n, LikelyPartialCountPerEq));
40

41
    // Fill the param id to index map
42
    std::map<uint32_t, int> paramToIndex;
43
    for(int j = 0; j < mat.n; j++) {
44
        paramToIndex[mat.param[j].v] = j;
45
    }
46

47
    if(mat.eq.size() >= MAX_UNKNOWNS) {
48
        return false;
49
    }
50
    std::vector<hParam> paramsUsed;
51
    // In some experimenting, this is almost always the right size.
52
    // Value is usually between 0 and 20, comes from number of constraints?
53
    mat.B.sym.reserve(mat.eq.size());
54
    for(size_t i = 0; i < mat.eq.size(); i++) {
55
        Equation *e = mat.eq[i];
56
        if(e->tag != tag) continue;
57
        // Simplify (fold) then deep-copy the current equation.
58
        Expr *f = e->e->FoldConstants();
59
        f = f->DeepCopyWithParamsAsPointers(&param, &(SK.param));
60

61
        paramsUsed.clear();
62
        f->ParamsUsedList(&paramsUsed);
63

64
        for(hParam &p : paramsUsed) {
65
            // Find the index of this parameter
66
            auto it = paramToIndex.find(p.v);
67
            if(it == paramToIndex.end()) continue;
68
            // this is the parameter index
69
            const int j = it->second;
70
            // compute partial derivative of f
71
            Expr *pd = f->PartialWrt(p);
72
            pd = pd->FoldConstants();
73
            if(pd->IsZeroConst())
74
                continue;
75
            mat.A.sym.insert(i, j) = pd;
76
        }
77
        paramsUsed.clear();
78
        mat.B.sym.push_back(f);
79
    }
80
    return true;
81
}
82

83
void System::EvalJacobian() {
84
    using namespace Eigen;
85
    mat.A.num.setZero();
86
    mat.A.num.resize(mat.m, mat.n);
87
    const int size = mat.A.sym.outerSize();
88

89
    for(int k = 0; k < size; k++) {
90
        for(SparseMatrix <Expr *>::InnerIterator it(mat.A.sym, k); it; ++it) {
91
            double value = it.value()->Eval();
92
            if(EXACT(value == 0.0)) continue;
93
            mat.A.num.insert(it.row(), it.col()) = value;
94
        }
95
    }
96
    mat.A.num.makeCompressed();
97
}
98

99
bool System::IsDragged(hParam p) {
100
    const auto b = dragged.begin();
101
    const auto e = dragged.end();
102
    return e != std::find(b, e, p);
103
}
104

105
Param *System::GetLastParamSubstitution(Param *p) {
106
    Param *current = p;
107
    while(current->substd != NULL) {
108
        current = current->substd;
109
        if(current == p) {
110
            // Break the loop
111
            current->substd = NULL;
112
            break;
113
        }
114
    }
115
    return current;
116
}
117

118
void System::SortSubstitutionByDragged(Param *p) {
119
    std::vector<Param *> subsParams;
120
    Param *by = NULL;
121
    Param *current = p;
122
    while(current != NULL) {
123
        subsParams.push_back(current);
124
        if(IsDragged(current->h)) {
125
            by = current;
126
        }
127
        current = current->substd;
128
    }
129
    if(by == NULL) by = p;
130
    for(Param *p : subsParams) {
131
       if(p == by) continue;
132
       p->substd = by;
133
       p->tag = VAR_SUBSTITUTED;
134
    }
135
    by->substd = NULL;
136
    by->tag = 0;
137
}
138

139
void System::SubstituteParamsByLast(Expr *e) {
140
    ssassert(e->op != Expr::Op::PARAM_PTR, "Expected an expression that refer to params via handles");
141

142
    if(e->op == Expr::Op::PARAM) {
143
        Param *p = param.FindByIdNoOops(e->parh);
144
        if(p != NULL) {
145
            Param *s = GetLastParamSubstitution(p);
146
            if(s != NULL) {
147
                e->parh = s->h;
148
            }
149
        }
150
    } else {
151
        int c = e->Children();
152
        if(c >= 1) {
153
            SubstituteParamsByLast(e->a);
154
            if(c >= 2) SubstituteParamsByLast(e->b);
155
        }
156
    }
157
}
158

159
void System::SolveBySubstitution() {
160
    for(auto &teq : eq) {
161
        Expr *tex = teq.e;
162

163
        if(tex->op    == Expr::Op::MINUS &&
164
           tex->a->op == Expr::Op::PARAM &&
165
           tex->b->op == Expr::Op::PARAM)
166
        {
167
            hParam a = tex->a->parh;
168
            hParam b = tex->b->parh;
169
            if(!(param.FindByIdNoOops(a) && param.FindByIdNoOops(b))) {
170
                // Don't substitute unless they're both solver params;
171
                // otherwise it's an equation that can be solved immediately,
172
                // or an error to flag later.
173
                continue;
174
            }
175

176
            if(a.v == b.v) {
177
                teq.tag = EQ_SUBSTITUTED;
178
                continue;
179
            }
180

181
            Param *pa = param.FindById(a);
182
            Param *pb = param.FindById(b);
183

184
            // Take the last substitution of parameter a
185
            // This resulted in creation of substitution chains
186
            Param *last = GetLastParamSubstitution(pa);
187
            last->substd = pb;
188
            last->tag = VAR_SUBSTITUTED;
189

190
            if(pb->substd != NULL) {
191
                // Break the loops
192
                GetLastParamSubstitution(pb);
193
                // if b loop was broken
194
                if(pb->substd == NULL) {
195
                    // Clear substitution
196
                    pb->tag = 0;
197
                }
198
            }
199
            teq.tag = EQ_SUBSTITUTED;
200
        }
201
    }
202

203
    //
204
    for(Param &p : param) {
205
        SortSubstitutionByDragged(&p);
206
    }
207

208
    // Substitute all the equations
209
    for(auto &req : eq) {
210
        SubstituteParamsByLast(req.e);
211
    }
212

213
    // Substitute all the parameters with last substitutions
214
    for(auto &p : param) {
215
        if(p.substd == NULL) continue;
216
        p.substd = GetLastParamSubstitution(p.substd);
217
    }
218
}
219

220
//-----------------------------------------------------------------------------
221
// Calculate the rank of the Jacobian matrix
222
//-----------------------------------------------------------------------------
223
int System::CalculateRank() {
224
    using namespace Eigen;
225
    if(mat.n == 0 || mat.m == 0) return 0;
226
    SparseQR <SparseMatrix<double>, COLAMDOrdering<int>> solver;
227
    solver.compute(mat.A.num);
228
    int result = solver.rank();
229
    return result;
230
}
231

232
bool System::TestRank(int *dof) {
233
    EvalJacobian();
234
    int jacobianRank = CalculateRank();
235
    // We are calculating dof based on real rank, not mat.m.
236
    // Using this approach we can calculate real dof even when redundant is allowed.
237
    if(dof != NULL) *dof = mat.n - jacobianRank;
238
    return jacobianRank == mat.m;
239
}
240

241
bool System::SolveLinearSystem(const Eigen::SparseMatrix <double> &A,
242
                               const Eigen::VectorXd &B, Eigen::VectorXd *X)
243
{
244
    if(A.outerSize() == 0) return true;
245
    using namespace Eigen;
246
    SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> solver;
247
    //SimplicialLDLT<SparseMatrix<double>> solver;
248
    solver.compute(A);
249
    *X = solver.solve(B);
250
    return (solver.info() == Success);
251
}
252

253
bool System::SolveLeastSquares() {
254
    using namespace Eigen;
255
    // Scale the columns; this scale weights the parameters for the least
256
    // squares solve, so that we can encourage the solver to make bigger
257
    // changes in some parameters, and smaller in others.
258
    mat.scale = VectorXd::Ones(mat.n);
259
    for(int c = 0; c < mat.n; c++) {
260
        if(IsDragged(mat.param[c])) {
261
            // It's least squares, so this parameter doesn't need to be all
262
            // that big to get a large effect.
263
            mat.scale[c] = 1 / 20.0;
264
        }
265
    }
266

267
    const int size = mat.A.num.outerSize();
268
    for(int k = 0; k < size; k++) {
269
        for(SparseMatrix<double>::InnerIterator it(mat.A.num, k); it; ++it) {
270
            it.valueRef() *= mat.scale[it.col()];
271
        }
272
    }
273

274
    SparseMatrix<double> AAt = mat.A.num * mat.A.num.transpose();
275
    AAt.makeCompressed();
276
    VectorXd z(mat.n);
277

278
    if(!SolveLinearSystem(AAt, mat.B.num, &z)) return false;
279

280
    mat.X = mat.A.num.transpose() * z;
281

282
    for(int c = 0; c < mat.n; c++) {
283
        mat.X[c] *= mat.scale[c];
284
    }
285
    return true;
286
}
287

288
bool System::NewtonSolve(int tag) {
289

290
    int iter = 0;
291
    bool converged = false;
292
    int i;
293

294
    // Evaluate the functions at our operating point.
295
    mat.B.num = Eigen::VectorXd(mat.m);
296
    for(i = 0; i < mat.m; i++) {
297
        mat.B.num[i] = (mat.B.sym[i])->Eval();
298
    }
299
    do {
300
        // And evaluate the Jacobian at our initial operating point.
301
        EvalJacobian();
302

303
        if(!SolveLeastSquares()) break;
304

305
        // Take the Newton step;
306
        //      J(x_n) (x_{n+1} - x_n) = 0 - F(x_n)
307
        for(i = 0; i < mat.n; i++) {
308
            Param *p = param.FindById(mat.param[i]);
309
            p->val -= mat.X[i];
310
            if(IsReasonable(p->val)) {
311
                // Very bad, and clearly not convergent
312
                return false;
313
            }
314
        }
315

316
        // Re-evalute the functions, since the params have just changed.
317
        for(i = 0; i < mat.m; i++) {
318
            mat.B.num[i] = (mat.B.sym[i])->Eval();
319
        }
320
        // Check for convergence
321
        converged = true;
322
        for(i = 0; i < mat.m; i++) {
323
            if(IsReasonable(mat.B.num[i])) {
324
                return false;
325
            }
326
            if(fabs(mat.B.num[i]) > CONVERGE_TOLERANCE) {
327
                converged = false;
328
                break;
329
            }
330
        }
331
    } while(iter++ < 50 && !converged);
332

333
    return converged;
334
}
335

336
void System::WriteEquationsExceptFor(hConstraint hc, Group *g) {
337
    // Generate all the equations from constraints in this group
338
    for(auto &con : SK.constraint) {
339
        ConstraintBase *c = &con;
340
        if(c->group != g->h) continue;
341
        if(c->h == hc) continue;
342

343
        if(c->HasLabel() && c->type != Constraint::Type::COMMENT &&
344
                g->allDimsReference)
345
        {
346
            // When all dimensions are reference, we adjust them to display
347
            // the correct value, and then don't generate any equations.
348
            c->ModifyToSatisfy();
349
            continue;
350
        }
351
        if(g->relaxConstraints && c->type != Constraint::Type::POINTS_COINCIDENT) {
352
            // When the constraints are relaxed, we keep only the point-
353
            // coincident constraints, and the constraints generated by
354
            // the entities and groups.
355
            continue;
356
        }
357

358
        c->GenerateEquations(&eq);
359
    }
360
    // And the equations from entities
361
    for(auto &ent : SK.entity) {
362
        EntityBase *e = &ent;
363
        if(e->group != g->h) continue;
364

365
        e->GenerateEquations(&eq);
366
    }
367
    // And from the groups themselves
368
    g->GenerateEquations(&eq);
369
}
370

371
void System::FindWhichToRemoveToFixJacobian(Group *g, List<hConstraint> *bad, bool forceDofCheck) {
372
    auto time = GetMilliseconds();
373
    g->solved.timeout = false;
374
    int a;
375

376
    for(a = 0; a < 2; a++) {
377
        for(auto &con : SK.constraint) {
378
            if((GetMilliseconds() - time) > g->solved.findToFixTimeout) {
379
                g->solved.timeout = true;
380
                return;
381
            }
382

383
            ConstraintBase *c = &con;
384
            if(c->group != g->h) continue;
385
            if((c->type == Constraint::Type::POINTS_COINCIDENT && a == 0) ||
386
               (c->type != Constraint::Type::POINTS_COINCIDENT && a == 1))
387
            {
388
                // Do the constraints in two passes: first everything but
389
                // the point-coincident constraints, then only those
390
                // constraints (so they appear last in the list).
391
                continue;
392
            }
393

394
            param.ClearTags();
395
            eq.Clear();
396
            WriteEquationsExceptFor(c->h, g);
397
            eq.ClearTags();
398

399
            // It's a major speedup to solve the easy ones by substitution here,
400
            // and that doesn't break anything.
401
            if(!forceDofCheck) {
402
                SolveBySubstitution();
403
            }
404

405
            WriteJacobian(0);
406
            EvalJacobian();
407

408
            int rank = CalculateRank();
409
            if(rank == mat.m) {
410
                // We fixed it by removing this constraint
411
                bad->Add(&(c->h));
412
            }
413
        }
414
    }
415
}
416

417
SolveResult System::Solve(Group *g, int *rank, int *dof, List<hConstraint> *bad,
418
                          bool andFindBad, bool andFindFree, bool forceDofCheck)
419
{
420
    WriteEquationsExceptFor(Constraint::NO_CONSTRAINT, g);
421

422
    bool rankOk;
423

424
/*
425
    int x;
426
    dbp("%d equations", eq.n);
427
    for(x = 0; x < eq.n; x++) {
428
        dbp("  %.3f = %s = 0", eq[x].e->Eval(), eq[x].e->Print());
429
    }
430
    dbp("%d parameters", param.n);
431
    for(x = 0; x < param.n; x++) {
432
        dbp("   param %08x at %.3f", param[x].h.v, param[x].val);
433
    } */
434

435
    // All params and equations are assigned to group zero.
436
    param.ClearTags();
437
    eq.ClearTags();
438

439
    // Since we are suppressing dof calculation or allowing redundant, we
440
    // can't / don't want to catch result of dof checking without substitution
441
    if(g->suppressDofCalculation || g->allowRedundant || !forceDofCheck) {
442
        SolveBySubstitution();
443
    }
444

445
    // Before solving the big system, see if we can find any equations that
446
    // are soluble alone. This can be a huge speedup. We don't know whether
447
    // the system is consistent yet, but if it isn't then we'll catch that
448
    // later.
449
    int alone = 1;
450
    for(auto &e : eq) {
451
        if(e.tag != 0)
452
            continue;
453

454
        hParam hp = e.e->ReferencedParams(&param);
455
        if(hp == Expr::NO_PARAMS) continue;
456
        if(hp == Expr::MULTIPLE_PARAMS) continue;
457

458
        Param *p = param.FindById(hp);
459
        if(p->tag != 0) continue; // let rank test catch inconsistency
460

461
        e.tag  = alone;
462
        p->tag = alone;
463
        WriteJacobian(alone);
464
        if(!NewtonSolve(alone)) {
465
            // We don't do the rank test, so let's arbitrarily return
466
            // the DIDNT_CONVERGE result here.
467
            rankOk = true;
468
            // Failed to converge, bail out early
469
            goto didnt_converge;
470
        }
471
        alone++;
472
    }
473

474
    // Now write the Jacobian for what's left, and do a rank test; that
475
    // tells us if the system is inconsistently constrained.
476
    if(!WriteJacobian(0)) {
477
        return SolveResult::TOO_MANY_UNKNOWNS;
478
    }
479
    // Clear dof value in order to have indication when dof is actually not calculated
480
    if(dof != NULL) *dof = -1;
481
    // We are suppressing or allowing redundant, so we no need to catch unsolveable + redundant
482
    rankOk = (!g->suppressDofCalculation && !g->allowRedundant) ? TestRank(dof) : true;
483

484
    // And do the leftovers as one big system
485
    if(!NewtonSolve(0)) {
486
        goto didnt_converge;
487
    }
488

489
    // Here we are want to calculate dof even when redundant is allowed, so just handle suppressing
490
    rankOk = (!g->suppressDofCalculation) ? TestRank(dof) : true;
491
    if(!rankOk) {
492
        if(andFindBad) FindWhichToRemoveToFixJacobian(g, bad, forceDofCheck);
493
    } else {
494
        MarkParamsFree(andFindFree);
495
    }
496
    // System solved correctly, so write the new values back in to the
497
    // main parameter table.
498
    for(auto &p : param) {
499
        double val;
500
        if(p.tag == VAR_SUBSTITUTED) {
501
            val = p.substd->val;
502
        } else {
503
            val = p.val;
504
        }
505
        Param *pp = SK.GetParam(p.h);
506
        pp->val = val;
507
        pp->known = true;
508
        pp->free  = p.free;
509
    }
510
    return rankOk ? SolveResult::OKAY : SolveResult::REDUNDANT_OKAY;
511

512
didnt_converge:
513
    SK.constraint.ClearTags();
514
    // Not using range-for here because index is used in additional ways
515
    for(size_t i = 0; i < mat.eq.size(); i++) {
516
        if(fabs(mat.B.num[i]) > CONVERGE_TOLERANCE || IsReasonable(mat.B.num[i])) {
517
            // This constraint is unsatisfied.
518
            if(!mat.eq[i]->h.isFromConstraint()) continue;
519

520
            hConstraint hc = mat.eq[i]->h.constraint();
521
            ConstraintBase *c = SK.constraint.FindByIdNoOops(hc);
522
            if(!c) continue;
523
            // Don't double-show constraints that generated multiple
524
            // unsatisfied equations
525
            if(!c->tag) {
526
                bad->Add(&(c->h));
527
                c->tag = 1;
528
            }
529
        }
530
    }
531

532
    return rankOk ? SolveResult::DIDNT_CONVERGE : SolveResult::REDUNDANT_DIDNT_CONVERGE;
533
}
534

535
SolveResult System::SolveRank(Group *g, int *rank, int *dof, List<hConstraint> *bad,
536
                              bool andFindBad, bool andFindFree)
537
{
538
    WriteEquationsExceptFor(Constraint::NO_CONSTRAINT, g);
539

540
    // All params and equations are assigned to group zero.
541
    param.ClearTags();
542
    eq.ClearTags();
543

544
    // Now write the Jacobian, and do a rank test; that
545
    // tells us if the system is inconsistently constrained.
546
    if(!WriteJacobian(0)) {
547
        return SolveResult::TOO_MANY_UNKNOWNS;
548
    }
549

550
    bool rankOk = TestRank(dof);
551
    if(!rankOk) {
552
        // When we are testing with redundant allowed, we don't want to have additional info
553
        // about redundants since this test is working only for single redundant constraint
554
        if(!g->suppressDofCalculation && !g->allowRedundant) {
555
            if(andFindBad) FindWhichToRemoveToFixJacobian(g, bad, true);
556
        }
557
    } else {
558
        MarkParamsFree(andFindFree);
559
    }
560
    return rankOk ? SolveResult::OKAY : SolveResult::REDUNDANT_OKAY;
561
}
562

563
void System::Clear() {
564
    entity.Clear();
565
    param.Clear();
566
    eq.Clear();
567
    dragged.Clear();
568
    mat.A.num.setZero();
569
    mat.A.sym.setZero();
570
}
571

572
void System::MarkParamsFree(bool find) {
573
    // If requested, find all the free (unbound) variables. This might be
574
    // more than the number of degrees of freedom. Don't always do this,
575
    // because the display would get annoying and it's slow.
576
    for(auto &p : param) {
577
        p.free = false;
578

579
        if(find) {
580
            if(p.tag == 0) {
581
                p.tag = VAR_DOF_TEST;
582
                WriteJacobian(0);
583
                EvalJacobian();
584
                int rank = CalculateRank();
585
                if(rank == mat.m) {
586
                    p.free = true;
587
                }
588
                p.tag = 0;
589
            }
590
        }
591
    }
592
}
593

594