diff --git a/entity.cpp b/entity.cpp index bfef455c..922f9cc7 100644 --- a/entity.cpp +++ b/entity.cpp @@ -232,25 +232,6 @@ void Entity::PointGetExprsInWorkplane(hEntity wrkpl, Expr **u, Expr **v) { } } -bool Entity::PointIsLocked(void) { - // A point is locked if none of its coordinates get assumed. - if(type == POINT_IN_3D) { - if(SS.GetParam(param[0])->assumed) return false; - if(SS.GetParam(param[1])->assumed) return false; - if(SS.GetParam(param[2])->assumed) return false; - } else if(type == POINT_IN_2D) { - if(SS.GetParam(param[0])->assumed) return false; - if(SS.GetParam(param[1])->assumed) return false; - } else if(type == POINT_XFRMD) { - if(SS.GetParam(param[0])->assumed) return false; - if(SS.GetParam(param[1])->assumed) return false; - if(SS.GetParam(param[2])->assumed) return false; - } else { - oops(); - } - return true; -} - void Entity::LineDrawOrGetDistance(Vector a, Vector b) { if(dogd.drawing) { glBegin(GL_LINE_STRIP); @@ -339,7 +320,7 @@ void Entity::DrawOrGetDistance(int order) { Point2d pp = SS.GW.ProjectPoint(v); // Make a free point slightly easier to select, so that with // coincident points, we select the free one. - dogd.dmin = pp.DistanceTo(dogd.mp) - (PointIsLocked() ? 3 : 4); + dogd.dmin = pp.DistanceTo(dogd.mp) - 4; } break; } diff --git a/sketch.h b/sketch.h index 04e2ac73..a954ce8b 100644 --- a/sketch.h +++ b/sketch.h @@ -179,7 +179,6 @@ public: bool IsPoint(void); // Applies for any of the point types - bool PointIsLocked(void); Vector PointGetCoords(void); ExprVector PointGetExprs(void); void PointGetExprsInWorkplane(hEntity wrkpl, Expr **u, Expr **v); diff --git a/solvespace.h b/solvespace.h index aa67946e..d1d14669 100644 --- a/solvespace.h +++ b/solvespace.h @@ -103,6 +103,9 @@ public: // The corresponding parameter for each column hParam param[MAX_UNKNOWNS]; + // The desired permutation, when we are sorting by sensitivity. + int permutation[MAX_UNKNOWNS]; + bool bound[MAX_UNKNOWNS]; // We're solving AX = B @@ -111,7 +114,15 @@ public: Expr *sym[MAX_UNKNOWNS][MAX_UNKNOWNS]; double num[MAX_UNKNOWNS][MAX_UNKNOWNS]; } A; + + // Extra information about each unknown: whether it's being dragged + // or not (in which case it should get assumed preferentially), and + // the sensitivity used to decide the order in which things get + // assumed. + bool dragged[MAX_UNKNOWNS]; + double sens[MAX_UNKNOWNS]; double X[MAX_UNKNOWNS]; + struct { Expr *sym[MAX_UNKNOWNS]; double num[MAX_UNKNOWNS]; @@ -125,6 +136,9 @@ public: void WriteJacobian(int eqTag, int paramTag); void EvalJacobian(void); + void SortBySensitivity(void); + void MarkAsDragged(hParam p); + bool NewtonSolve(int tag); bool Solve(void); }; diff --git a/system.cpp b/system.cpp index f7e4e9d8..997ddffb 100644 --- a/system.cpp +++ b/system.cpp @@ -48,6 +48,91 @@ void System::EvalJacobian(void) { } } +void System::MarkAsDragged(hParam p) { + int j; + for(j = 0; j < mat.n; j++) { + if(mat.param[j].v == p.v) { + mat.dragged[j] = true; + } + } +} + +static int BySensitivity(const void *va, const void *vb) { + const int *a = (const int *)va, *b = (const int *)vb; + + if(SS.sys.mat.dragged[*a] && !SS.sys.mat.dragged[*b]) return 1; + if(SS.sys.mat.dragged[*b] && !SS.sys.mat.dragged[*a]) return -1; + + double as = SS.sys.mat.sens[*a]; + double bs = SS.sys.mat.sens[*b]; + + if(as < bs) return 1; + if(as > bs) return -1; + return 0; +} +void System::SortBySensitivity(void) { + // For each unknown, sum up the sensitivities in that column of the + // Jacobian + int i, j; + for(j = 0; j < mat.n; j++) { + double s = 0; + int i; + for(i = 0; i < mat.m; i++) { + s += fabs(mat.A.num[i][j]); + } + mat.sens[j] = s; + } + for(j = 0; j < mat.n; j++) { + mat.dragged[j] = false; + mat.permutation[j] = j; + } + if(SS.GW.pendingPoint.v) { + Entity *p = SS.GetEntity(SS.GW.pendingPoint); + switch(p->type) { + case Entity::POINT_IN_3D: + MarkAsDragged(p->param[0]); + MarkAsDragged(p->param[1]); + MarkAsDragged(p->param[2]); + break; + case Entity::POINT_IN_2D: + MarkAsDragged(p->param[0]); + MarkAsDragged(p->param[1]); + break; + default: oops(); + } + } + + qsort(mat.permutation, mat.n, sizeof(mat.permutation[0]), BySensitivity); + + int origPos[MAX_UNKNOWNS]; + int entryWithOrigPos[MAX_UNKNOWNS]; + for(j = 0; j < mat.n; j++) { + origPos[j] = j; + entryWithOrigPos[j] = j; + } + +#define SWAP(T, a, b) do { T temp = (a); (a) = (b); (b) = temp; } while(0) + for(j = 0; j < mat.n; j++) { + int dest = j; // we are writing to position j + // And the source is whichever position ahead of us can be swapped + // in to make the permutation vectors line up. + int src = entryWithOrigPos[mat.permutation[j]]; + + for(i = 0; i < mat.m; i++) { + SWAP(double, mat.A.num[i][src], mat.A.num[i][dest]); + SWAP(Expr *, mat.A.sym[i][src], mat.A.sym[i][dest]); + } + SWAP(hParam, mat.param[src], mat.param[dest]); + + SWAP(int, origPos[src], origPos[dest]); + if(mat.permutation[dest] != origPos[dest]) oops(); + + // Update the table; only necessary to do this for src, since dest + // is already done. + entryWithOrigPos[origPos[src]] = src; + } +} + bool System::Tol(double v) { return (fabs(v) < 0.001); } @@ -232,6 +317,8 @@ bool System::Solve(void) { WriteJacobian(0, 0); EvalJacobian(); + SortBySensitivity(); + /* dbp("write/eval jacboian=%d", GetMilliseconds() - in); for(i = 0; i < mat.m; i++) { dbp("function %d: %s", i, mat.B.sym[i]->Print()); @@ -251,7 +338,7 @@ bool System::Solve(void) { } */ // Fix any still-free variables wherever they are now. - for(j = 0; j < mat.n; j++) { + for(j = mat.n-1; j >= 0; --j) { if(mat.bound[j]) continue; Param *p = param.FindByIdNoOops(mat.param[j]); if(!p) {