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 // This tolerance is used to determine whether two (linearized) constraints
12 // are linearly dependent. If this is too small, then we will attempt to
13 // solve truly inconsistent systems and fail. But if it's too large, then
14 // we will give up on legitimate systems like a skinny right angle triangle by
15 // its hypotenuse and long side.
16 const double System::RANK_MAG_TOLERANCE = 1e-4;
17
18 // The solver will converge all unknowns to within this tolerance. This must
19 // always be much less than LENGTH_EPS, and in practice should be much less.
20 const double System::CONVERGE_TOLERANCE = (LENGTH_EPS/(1e2));
21
WriteJacobian(int tag)22 bool System::WriteJacobian(int tag) {
23 int a, i, j;
24
25 j = 0;
26 for(a = 0; a < param.n; a++) {
27 if(j >= MAX_UNKNOWNS) return false;
28
29 Param *p = &(param.elem[a]);
30 if(p->tag != tag) continue;
31 mat.param[j] = p->h;
32 j++;
33 }
34 mat.n = j;
35
36 i = 0;
37 for(a = 0; a < eq.n; a++) {
38 if(i >= MAX_UNKNOWNS) return false;
39
40 Equation *e = &(eq.elem[a]);
41 if(e->tag != tag) continue;
42
43 mat.eq[i] = e->h;
44 Expr *f = e->e->DeepCopyWithParamsAsPointers(¶m, &(SK.param));
45 f = f->FoldConstants();
46
47 // Hash table (61 bits) to accelerate generation of zero partials.
48 uint64_t scoreboard = f->ParamsUsed();
49 for(j = 0; j < mat.n; j++) {
50 Expr *pd;
51 if(scoreboard & ((uint64_t)1 << (mat.param[j].v % 61)) &&
52 f->DependsOn(mat.param[j]))
53 {
54 pd = f->PartialWrt(mat.param[j]);
55 pd = pd->FoldConstants();
56 pd = pd->DeepCopyWithParamsAsPointers(¶m, &(SK.param));
57 } else {
58 pd = Expr::From(0.0);
59 }
60 mat.A.sym[i][j] = pd;
61 }
62 mat.B.sym[i] = f;
63 i++;
64 }
65 mat.m = i;
66
67 return true;
68 }
69
EvalJacobian(void)70 void System::EvalJacobian(void) {
71 int i, j;
72 for(i = 0; i < mat.m; i++) {
73 for(j = 0; j < mat.n; j++) {
74 mat.A.num[i][j] = (mat.A.sym[i][j])->Eval();
75 }
76 }
77 }
78
IsDragged(hParam p)79 bool System::IsDragged(hParam p) {
80 hParam *pp;
81 for(pp = dragged.First(); pp; pp = dragged.NextAfter(pp)) {
82 if(p.v == pp->v) return true;
83 }
84 return false;
85 }
86
SolveBySubstitution(void)87 void System::SolveBySubstitution(void) {
88 int i;
89 for(i = 0; i < eq.n; i++) {
90 Equation *teq = &(eq.elem[i]);
91 Expr *tex = teq->e;
92
93 if(tex->op == Expr::MINUS &&
94 tex->a->op == Expr::PARAM &&
95 tex->b->op == Expr::PARAM)
96 {
97 hParam a = tex->a->parh;
98 hParam b = tex->b->parh;
99 if(!(param.FindByIdNoOops(a) && param.FindByIdNoOops(b))) {
100 // Don't substitute unless they're both solver params;
101 // otherwise it's an equation that can be solved immediately,
102 // or an error to flag later.
103 continue;
104 }
105
106 if(IsDragged(a)) {
107 // A is being dragged, so A should stay, and B should go
108 hParam t = a;
109 a = b;
110 b = t;
111 }
112
113 int j;
114 for(j = 0; j < eq.n; j++) {
115 Equation *req = &(eq.elem[j]);
116 (req->e)->Substitute(a, b); // A becomes B, B unchanged
117 }
118 for(j = 0; j < param.n; j++) {
119 Param *rp = &(param.elem[j]);
120 if(rp->substd.v == a.v) {
121 rp->substd = b;
122 }
123 }
124 Param *ptr = param.FindById(a);
125 ptr->tag = VAR_SUBSTITUTED;
126 ptr->substd = b;
127
128 teq->tag = EQ_SUBSTITUTED;
129 }
130 }
131 }
132
133 //-----------------------------------------------------------------------------
134 // Calculate the rank of the Jacobian matrix, by Gram-Schimdt orthogonalization
135 // in place. A row (~equation) is considered to be all zeros if its magnitude
136 // is less than the tolerance RANK_MAG_TOLERANCE.
137 //-----------------------------------------------------------------------------
CalculateRank(void)138 int System::CalculateRank(void) {
139 // Actually work with magnitudes squared, not the magnitudes
140 double rowMag[MAX_UNKNOWNS] = {};
141 double tol = RANK_MAG_TOLERANCE*RANK_MAG_TOLERANCE;
142
143 int i, iprev, j;
144 int rank = 0;
145
146 for(i = 0; i < mat.m; i++) {
147 // Subtract off this row's component in the direction of any
148 // previous rows
149 for(iprev = 0; iprev < i; iprev++) {
150 if(rowMag[iprev] <= tol) continue; // ignore zero rows
151
152 double dot = 0;
153 for(j = 0; j < mat.n; j++) {
154 dot += (mat.A.num[iprev][j]) * (mat.A.num[i][j]);
155 }
156 for(j = 0; j < mat.n; j++) {
157 mat.A.num[i][j] -= (dot/rowMag[iprev])*mat.A.num[iprev][j];
158 }
159 }
160 // Our row is now normal to all previous rows; calculate the
161 // magnitude of what's left
162 double mag = 0;
163 for(j = 0; j < mat.n; j++) {
164 mag += (mat.A.num[i][j]) * (mat.A.num[i][j]);
165 }
166 if(mag > tol) {
167 rank++;
168 }
169 rowMag[i] = mag;
170 }
171
172 return rank;
173 }
174
TestRank(void)175 bool System::TestRank(void) {
176 EvalJacobian();
177 return CalculateRank() == mat.m;
178 }
179
SolveLinearSystem(double X[],double A[][MAX_UNKNOWNS],double B[],int n)180 bool System::SolveLinearSystem(double X[], double A[][MAX_UNKNOWNS],
181 double B[], int n)
182 {
183 // Gaussian elimination, with partial pivoting. It's an error if the
184 // matrix is singular, because that means two constraints are
185 // equivalent.
186 int i, j, ip, jp, imax = 0;
187 double max, temp;
188
189 for(i = 0; i < n; i++) {
190 // We are trying eliminate the term in column i, for rows i+1 and
191 // greater. First, find a pivot (between rows i and N-1).
192 max = 0;
193 for(ip = i; ip < n; ip++) {
194 if(ffabs(A[ip][i]) > max) {
195 imax = ip;
196 max = ffabs(A[ip][i]);
197 }
198 }
199 // Don't give up on a singular matrix unless it's really bad; the
200 // assumption code is responsible for identifying that condition,
201 // so we're not responsible for reporting that error.
202 if(ffabs(max) < 1e-20) continue;
203
204 // Swap row imax with row i
205 for(jp = 0; jp < n; jp++) {
206 swap(A[i][jp], A[imax][jp]);
207 }
208 swap(B[i], B[imax]);
209
210 // For rows i+1 and greater, eliminate the term in column i.
211 for(ip = i+1; ip < n; ip++) {
212 temp = A[ip][i]/A[i][i];
213
214 for(jp = i; jp < n; jp++) {
215 A[ip][jp] -= temp*(A[i][jp]);
216 }
217 B[ip] -= temp*B[i];
218 }
219 }
220
221 // We've put the matrix in upper triangular form, so at this point we
222 // can solve by back-substitution.
223 for(i = n - 1; i >= 0; i--) {
224 if(ffabs(A[i][i]) < 1e-20) continue;
225
226 temp = B[i];
227 for(j = n - 1; j > i; j--) {
228 temp -= X[j]*A[i][j];
229 }
230 X[i] = temp / A[i][i];
231 }
232
233 return true;
234 }
235
SolveLeastSquares(void)236 bool System::SolveLeastSquares(void) {
237 int r, c, i;
238
239 // Scale the columns; this scale weights the parameters for the least
240 // squares solve, so that we can encourage the solver to make bigger
241 // changes in some parameters, and smaller in others.
242 for(c = 0; c < mat.n; c++) {
243 if(IsDragged(mat.param[c])) {
244 // It's least squares, so this parameter doesn't need to be all
245 // that big to get a large effect.
246 mat.scale[c] = 1/20.0;
247 } else {
248 mat.scale[c] = 1;
249 }
250 for(r = 0; r < mat.m; r++) {
251 mat.A.num[r][c] *= mat.scale[c];
252 }
253 }
254
255 // Write A*A'
256 for(r = 0; r < mat.m; r++) {
257 for(c = 0; c < mat.m; c++) { // yes, AAt is square
258 double sum = 0;
259 for(i = 0; i < mat.n; i++) {
260 sum += mat.A.num[r][i]*mat.A.num[c][i];
261 }
262 mat.AAt[r][c] = sum;
263 }
264 }
265
266 if(!SolveLinearSystem(mat.Z, mat.AAt, mat.B.num, mat.m)) return false;
267
268 // And multiply that by A' to get our solution.
269 for(c = 0; c < mat.n; c++) {
270 double sum = 0;
271 for(i = 0; i < mat.m; i++) {
272 sum += mat.A.num[i][c]*mat.Z[i];
273 }
274 mat.X[c] = sum * mat.scale[c];
275 }
276 return true;
277 }
278
NewtonSolve(int tag)279 bool System::NewtonSolve(int tag) {
280
281 int iter = 0;
282 bool converged = false;
283 int i;
284
285 // Evaluate the functions at our operating point.
286 for(i = 0; i < mat.m; i++) {
287 mat.B.num[i] = (mat.B.sym[i])->Eval();
288 }
289 do {
290 // And evaluate the Jacobian at our initial operating point.
291 EvalJacobian();
292
293 if(!SolveLeastSquares()) break;
294
295 // Take the Newton step;
296 // J(x_n) (x_{n+1} - x_n) = 0 - F(x_n)
297 for(i = 0; i < mat.n; i++) {
298 Param *p = param.FindById(mat.param[i]);
299 p->val -= mat.X[i];
300 if(isnan(p->val)) {
301 // Very bad, and clearly not convergent
302 return false;
303 }
304 }
305
306 // Re-evalute the functions, since the params have just changed.
307 for(i = 0; i < mat.m; i++) {
308 mat.B.num[i] = (mat.B.sym[i])->Eval();
309 }
310 // Check for convergence
311 converged = true;
312 for(i = 0; i < mat.m; i++) {
313 if(isnan(mat.B.num[i])) {
314 return false;
315 }
316 if(ffabs(mat.B.num[i]) > CONVERGE_TOLERANCE) {
317 converged = false;
318 break;
319 }
320 }
321 } while(iter++ < 50 && !converged);
322
323 return converged;
324 }
325
WriteEquationsExceptFor(hConstraint hc,Group * g)326 void System::WriteEquationsExceptFor(hConstraint hc, Group *g) {
327 int i;
328 // Generate all the equations from constraints in this group
329 for(i = 0; i < SK.constraint.n; i++) {
330 ConstraintBase *c = &(SK.constraint.elem[i]);
331 if(c->group.v != g->h.v) continue;
332 if(c->h.v == hc.v) continue;
333
334 if(c->HasLabel() && c->type != Constraint::COMMENT &&
335 g->allDimsReference)
336 {
337 // When all dimensions are reference, we adjust them to display
338 // the correct value, and then don't generate any equations.
339 c->ModifyToSatisfy();
340 continue;
341 }
342 if(g->relaxConstraints && c->type != Constraint::POINTS_COINCIDENT) {
343 // When the constraints are relaxed, we keep only the point-
344 // coincident constraints, and the constraints generated by
345 // the entities and groups.
346 continue;
347 }
348
349 c->Generate(&eq);
350 }
351 // And the equations from entities
352 for(i = 0; i < SK.entity.n; i++) {
353 EntityBase *e = &(SK.entity.elem[i]);
354 if(e->group.v != g->h.v) continue;
355
356 e->GenerateEquations(&eq);
357 }
358 // And from the groups themselves
359 g->GenerateEquations(&eq);
360 }
361
FindWhichToRemoveToFixJacobian(Group * g,List<hConstraint> * bad)362 void System::FindWhichToRemoveToFixJacobian(Group *g, List<hConstraint> *bad) {
363 int a, i;
364
365 for(a = 0; a < 2; a++) {
366 for(i = 0; i < SK.constraint.n; i++) {
367 ConstraintBase *c = &(SK.constraint.elem[i]);
368 if(c->group.v != g->h.v) continue;
369 if((c->type == Constraint::POINTS_COINCIDENT && a == 0) ||
370 (c->type != Constraint::POINTS_COINCIDENT && a == 1))
371 {
372 // Do the constraints in two passes: first everything but
373 // the point-coincident constraints, then only those
374 // constraints (so they appear last in the list).
375 continue;
376 }
377
378 param.ClearTags();
379 eq.Clear();
380 WriteEquationsExceptFor(c->h, g);
381 eq.ClearTags();
382
383 // It's a major speedup to solve the easy ones by substitution here,
384 // and that doesn't break anything.
385 SolveBySubstitution();
386
387 WriteJacobian(0);
388 EvalJacobian();
389
390 int rank = CalculateRank();
391 if(rank == mat.m) {
392 // We fixed it by removing this constraint
393 bad->Add(&(c->h));
394 }
395 }
396 }
397 }
398
Solve(Group * g,int * dof,List<hConstraint> * bad,bool andFindBad,bool andFindFree)399 int System::Solve(Group *g, int *dof, List<hConstraint> *bad,
400 bool andFindBad, bool andFindFree)
401 {
402 WriteEquationsExceptFor(Constraint::NO_CONSTRAINT, g);
403
404 int i;
405 bool rankOk;
406
407 /*
408 dbp("%d equations", eq.n);
409 for(i = 0; i < eq.n; i++) {
410 dbp(" %.3f = %s = 0", eq.elem[i].e->Eval(), eq.elem[i].e->Print());
411 }
412 dbp("%d parameters", param.n);
413 for(i = 0; i < param.n; i++) {
414 dbp(" param %08x at %.3f", param.elem[i].h.v, param.elem[i].val);
415 } */
416
417 // All params and equations are assigned to group zero.
418 param.ClearTags();
419 eq.ClearTags();
420
421 SolveBySubstitution();
422
423 // Before solving the big system, see if we can find any equations that
424 // are soluble alone. This can be a huge speedup. We don't know whether
425 // the system is consistent yet, but if it isn't then we'll catch that
426 // later.
427 int alone = 1;
428 for(i = 0; i < eq.n; i++) {
429 Equation *e = &(eq.elem[i]);
430 if(e->tag != 0) continue;
431
432 hParam hp = e->e->ReferencedParams(¶m);
433 if(hp.v == Expr::NO_PARAMS.v) continue;
434 if(hp.v == Expr::MULTIPLE_PARAMS.v) continue;
435
436 Param *p = param.FindById(hp);
437 if(p->tag != 0) continue; // let rank test catch inconsistency
438
439 e->tag = alone;
440 p->tag = alone;
441 WriteJacobian(alone);
442 if(!NewtonSolve(alone)) {
443 // Failed to converge, bail out early
444 goto didnt_converge;
445 }
446 alone++;
447 }
448
449 // Now write the Jacobian for what's left, and do a rank test; that
450 // tells us if the system is inconsistently constrained.
451 if(!WriteJacobian(0)) {
452 return System::TOO_MANY_UNKNOWNS;
453 }
454
455 rankOk = TestRank();
456
457 // And do the leftovers as one big system
458 if(!NewtonSolve(0)) {
459 goto didnt_converge;
460 }
461
462 rankOk = TestRank();
463 if(!rankOk) {
464 if(!g->allowRedundant) {
465 if(andFindBad) FindWhichToRemoveToFixJacobian(g, bad);
466 }
467 } else {
468 // This is not the full Jacobian, but any substitutions or single-eq
469 // solves removed one equation and one unknown, therefore no effect
470 // on the number of DOF.
471 if(dof) *dof = mat.n - mat.m;
472
473 // If requested, find all the free (unbound) variables. This might be
474 // more than the number of degrees of freedom. Don't always do this,
475 // because the display would get annoying and it's slow.
476 for(i = 0; i < param.n; i++) {
477 Param *p = &(param.elem[i]);
478 p->free = false;
479
480 if(andFindFree) {
481 if(p->tag == 0) {
482 p->tag = VAR_DOF_TEST;
483 WriteJacobian(0);
484 EvalJacobian();
485 int rank = CalculateRank();
486 if(rank == mat.m) {
487 p->free = true;
488 }
489 p->tag = 0;
490 }
491 }
492 }
493 }
494 // System solved correctly, so write the new values back in to the
495 // main parameter table.
496 for(i = 0; i < param.n; i++) {
497 Param *p = &(param.elem[i]);
498 double val;
499 if(p->tag == VAR_SUBSTITUTED) {
500 val = param.FindById(p->substd)->val;
501 } else {
502 val = p->val;
503 }
504 Param *pp = SK.GetParam(p->h);
505 pp->val = val;
506 pp->known = true;
507 pp->free = p->free;
508 }
509 return rankOk ? System::SOLVED_OKAY : System::REDUNDANT_OKAY;
510
511 didnt_converge:
512 SK.constraint.ClearTags();
513 for(i = 0; i < eq.n; i++) {
514 if(ffabs(mat.B.num[i]) > CONVERGE_TOLERANCE || isnan(mat.B.num[i])) {
515 // This constraint is unsatisfied.
516 if(!mat.eq[i].isFromConstraint()) continue;
517
518 hConstraint hc = mat.eq[i].constraint();
519 ConstraintBase *c = SK.constraint.FindByIdNoOops(hc);
520 if(!c) continue;
521 // Don't double-show constraints that generated multiple
522 // unsatisfied equations
523 if(!c->tag) {
524 bad->Add(&(c->h));
525 c->tag = 1;
526 }
527 }
528 }
529
530 return rankOk ? System::DIDNT_CONVERGE : System::REDUNDANT_DIDNT_CONVERGE;
531 }
532
Clear(void)533 void System::Clear(void) {
534 entity.Clear();
535 param.Clear();
536 eq.Clear();
537 dragged.Clear();
538 }
539