From b78b10ac1a455bc332e889e5c33d277890343945 Mon Sep 17 00:00:00 2001 From: Jonathan Westhues Date: Sun, 20 Apr 2008 03:35:10 -0800 Subject: [PATCH] Ultra-rough beginnings of a solver. Write the constraint equations, take the partial derivatives, and run the Newton's method. This seems to sort of work with a single distance constraint. [git-p4: depot-paths = "//depot/solvespace/": change = 1675] --- Makefile | 1 + constraint.cpp | 54 ++++++++++- entity.cpp | 54 +++++++++++ graphicswin.cpp | 3 +- sketch.h | 34 ++++++- solvespace.cpp | 76 +++++++++++++++ solvespace.h | 50 ++++++++++ system.cpp | 244 ++++++++++++++++++++++++++++++++++++++++++++++++ ui.h | 1 + 9 files changed, 514 insertions(+), 3 deletions(-) create mode 100644 system.cpp diff --git a/Makefile b/Makefile index 9292934..494e58f 100644 --- a/Makefile +++ b/Makefile @@ -20,6 +20,7 @@ SSOBJS = $(OBJDIR)\solvespace.obj \ $(OBJDIR)\constraint.obj \ $(OBJDIR)\drawconstraint.obj \ $(OBJDIR)\file.obj \ + $(OBJDIR)\system.obj \ LIBS = user32.lib gdi32.lib comctl32.lib advapi32.lib opengl32.lib glu32.lib diff --git a/constraint.cpp b/constraint.cpp index de19898..8804718 100644 --- a/constraint.cpp +++ b/constraint.cpp @@ -29,13 +29,65 @@ void Constraint::MenuConstrain(int id) { return; } c.disp.offset = Vector::MakeFrom(50, 50, 50); - c.exprA = Expr::FromString("1+3+2")->DeepCopyKeep(); + c.exprA = Expr::FromString("300")->DeepCopyKeep(); AddConstraint(&c); break; + case GraphicsWindow::MNU_SOLVE_NOW: + SS.Solve(); + return; + default: oops(); } SS.GW.ClearSelection(); InvalidateGraphics(); } + +Expr *Constraint::Distance(hEntity hpa, hEntity hpb) { + Entity *pa = SS.GetEntity(hpa); + Entity *pb = SS.GetEntity(hpb); + + if(!(pa->IsPoint() && pb->IsPoint())) oops(); + + if(pa->type == Entity::POINT_IN_2D && + pb->type == Entity::POINT_IN_2D && + pa->csys.v == pb->csys.v) + { + // A nice case; they are both in the same 2d csys, so I can write + // the equation in terms of the basis vectors in that csys. + Expr *du = Expr::FromParam(pa->param.h[0])->Minus( + Expr::FromParam(pb->param.h[0])); + Expr *dv = Expr::FromParam(pa->param.h[1])->Minus( + Expr::FromParam(pb->param.h[1])); + + return ((du->Square())->Plus(dv->Square()))->Sqrt(); + } + Expr *ax, *ay, *az; + Expr *bx, *by, *bz; + pa->PointGetExprs(&ax, &ay, &az); + pb->PointGetExprs(&bx, &by, &bz); + + Expr *dx2 = (ax->Minus(bx))->Square(); + Expr *dy2 = (ay->Minus(by))->Square(); + Expr *dz2 = (az->Minus(bz))->Square(); + + return (dx2->Plus(dy2->Plus(dz2)))->Sqrt(); +} + +void Constraint::AddEq(IdList *l, Expr *expr, int index) { + Equation eq; + eq.e = expr; + eq.h = h.equation(index); + l->Add(&eq); +} + +void Constraint::Generate(IdList *l) { + switch(type) { + case PT_PT_DISTANCE: + AddEq(l, Distance(ptA, ptB)->Minus(exprA), 0); + break; + + default: oops(); + } +} diff --git a/entity.cpp b/entity.cpp index 57617c5..2a64628 100644 --- a/entity.cpp +++ b/entity.cpp @@ -16,6 +16,33 @@ void Entity::Csys2dGetBasisVectors(Vector *u, Vector *v) { *v = quat.RotationV(); } +void Entity::Csys2dGetBasisExprs(Expr **u, Expr **v) { + Expr *a = Expr::FromParam(param.h[0]); + Expr *b = Expr::FromParam(param.h[1]); + Expr *c = Expr::FromParam(param.h[2]); + Expr *d = Expr::FromParam(param.h[3]); + + Expr *two = Expr::FromConstant(2); + + u[0] = a->Square(); + u[0] = (u[0])->Plus(b->Square()); + u[0] = (u[0])->Minus(c->Square()); + u[0] = (u[0])->Minus(d->Square()); + u[1] = two->Times(a->Times(d)); + u[1] = (u[1])->Plus(two->Times(b->Times(c))); + u[2] = two->Times(b->Times(d)); + u[2] = (u[2])->Minus(two->Times(a->Times(c))); + + v[0] = two->Times(b->Times(c)); + v[0] = (v[0])->Minus(two->Times(a->Times(d))); + v[1] = a->Square(); + v[1] = (v[1])->Minus(b->Square()); + v[1] = (v[1])->Plus(c->Square()); + v[1] = (v[1])->Minus(d->Square()); + v[2] = two->Times(a->Times(b)); + v[2] = (v[2])->Plus(two->Times(c->Times(d))); +} + bool Entity::IsPoint(void) { switch(type) { case POINT_IN_3D: @@ -76,6 +103,33 @@ Vector Entity::PointGetCoords(void) { return p; } +void Entity::PointGetExprs(Expr **x, Expr **y, Expr **z) { + switch(type) { + case POINT_IN_3D: + *x = Expr::FromParam(param.h[0]); + *y = Expr::FromParam(param.h[1]); + *z = Expr::FromParam(param.h[2]); + break; + + case POINT_IN_2D: { + Entity *c = SS.GetEntity(csys); + Expr *u[3], *v[3]; + c->Csys2dGetBasisExprs(u, v); + + *x = Expr::FromParam(param.h[0])->Times(u[0]); + *x = (*x)->Plus(Expr::FromParam(param.h[1])->Times(v[0])); + + *y = Expr::FromParam(param.h[0])->Times(u[1]); + *y = (*y)->Plus(Expr::FromParam(param.h[1])->Times(v[1])); + + *z = Expr::FromParam(param.h[0])->Times(u[2]); + *z = (*z)->Plus(Expr::FromParam(param.h[1])->Times(v[2])); + break; + } + default: oops(); + } +} + void Entity::LineDrawOrGetDistance(Vector a, Vector b) { if(dogd.drawing) { glBegin(GL_LINE_STRIP); diff --git a/graphicswin.cpp b/graphicswin.cpp index 87455ce..38af51b 100644 --- a/graphicswin.cpp +++ b/graphicswin.cpp @@ -75,7 +75,8 @@ const GraphicsWindow::MenuEntry GraphicsWindow::menu[] = { { 1, "S&ymmetric\tShift+Y", 0, 'Y'|S, NULL }, { 1, NULL, 0, NULL }, { 1, "Sym&bolic Equation\tShift+B", 0, 'B'|S, NULL }, - +{ 1, NULL, 0, NULL }, +{ 1, "Solve Once Now\tSpace", MNU_SOLVE_NOW, ' ', mCon }, { 0, "&Help", 0, NULL }, { 1, "&About\t", 0, NULL }, diff --git a/sketch.h b/sketch.h index 67d24b8..fb1321e 100644 --- a/sketch.h +++ b/sketch.h @@ -53,13 +53,29 @@ public: int tag; hGroup h; + int solveOrder; + bool solved; + NameStr name; char *DescriptionString(void); }; + +class EntityId { + DWORD v; // entity ID, starting from 0 +}; +class EntityMap { + int tag; + + EntityId h; + hEntity input; + int copyNumber; + // (input, copyNumber) gets mapped to ((Request)xxx).entity(h.v) +}; + // A user request for some primitive or derived operation; for example a -// line, or a +// line, or a step and repeat. class Request { public: // Some predefined requests, that are present in every sketch. @@ -83,6 +99,13 @@ public: NameStr name; + // When a request generates entities from entities, and the source + // entities may have come from multiple requests, it's necessary to + // remap the entity ID so that it's still unique. We do this with a + // mapping list. + IdList remap; + hEntity Remap(hEntity in, int copyNumber); + hParam AddParam(IdList *param, hParam hp); void Generate(IdList *entity, IdList *param); @@ -116,6 +139,7 @@ public: // Applies only for a CSYS_2D type void Csys2dGetBasisVectors(Vector *u, Vector *v); + void Csys2dGetBasisExprs(Expr **u, Expr **v); bool IsPoint(void); // Applies for any of the point types @@ -166,6 +190,8 @@ inline hRequest hParam::request(void) class hConstraint { public: DWORD v; + + hEquation equation(int i); }; class Constraint { @@ -213,6 +239,9 @@ public: bool HasLabel(void); void Generate(IdList *l); + // Some helpers when generating symbolic constraint equations + void AddEq(IdList *l, Expr *expr, int index); + static Expr *Distance(hEntity pa, hEntity pb); }; class hEquation { @@ -228,5 +257,8 @@ public: Expr *e; }; +inline hEquation hConstraint::equation(int i) + { hEquation r; r.v = (v << 16) | i; return r; } + #endif diff --git a/solvespace.cpp b/solvespace.cpp index 0e7c6c1..fc89f83 100644 --- a/solvespace.cpp +++ b/solvespace.cpp @@ -67,6 +67,7 @@ void SolveSpace::GenerateAll(void) { } } + prev.Clear(); ForceReferences(); } @@ -94,7 +95,82 @@ void SolveSpace::ForceReferences(void) { } } +bool SolveSpace::SolveGroup(hGroup hg) { + int i; + if(hg.v == Group::HGROUP_REFERENCES.v) { + // Special case; mark everything in the references known. + for(i = 0; i < param.n; i++) { + Param *p = &(param.elem[i]); + Request *r = GetRequest(p->h.request()); + if(r->group.v == hg.v) p->known = true; + } + return true; + } + + // Clear out the system to be solved. + sys.entity.Clear(); + sys.param.Clear(); + sys.eq.Clear(); + // And generate all the params for requests in this group + for(i = 0; i < request.n; i++) { + Request *r = &(request.elem[i]); + if(r->group.v != hg.v) continue; + + r->Generate(&(sys.entity), &(sys.param)); + } + // Set the initial guesses for all the params + for(i = 0; i < sys.param.n; i++) { + Param *p = &(sys.param.elem[i]); + p->known = false; + p->val = GetParam(p->h)->val; + } + // And generate all the equations from constraints in this group + for(i = 0; i < constraint.n; i++) { + Constraint *c = &(constraint.elem[i]); + if(c->group.v != hg.v) continue; + + c->Generate(&(sys.eq)); + } + + return sys.Solve(); +} + +bool SolveSpace::SolveWorker(void) { + bool allSolved = true; + + int i; + for(i = 0; i < group.n; i++) { + Group *g = &(group.elem[i]); + if(g->solved) continue; + + allSolved = false; + dbp("try solve group %s", g->DescriptionString()); + if(SolveGroup(g->h)) { + g->solved = true; + // So this one worked; let's see if we can go any further. + if(SolveWorker()) { + // So everything worked; we're done. + return true; + } else { + // Didn't work, so undo this choice and give up + g->solved = false; + } + } + } + + // If we got here, then either everything failed, so we're stuck, or + // everything was already solved, so we're done. + return allSolved; +} + void SolveSpace::Solve(void) { + int i; + for(i = 0; i < group.n; i++) { + group.elem[i].solved = false; + } + SolveWorker(); + + InvalidateGraphics(); } void SolveSpace::MenuFile(int id) { diff --git a/solvespace.h b/solvespace.h index 4f416f1..5211f1c 100644 --- a/solvespace.h +++ b/solvespace.h @@ -73,6 +73,49 @@ void MakeMatrix(double *mat, double a11, double a12, double a13, double a14, double a31, double a32, double a33, double a34, double a41, double a42, double a43, double a44); +class System { +public: +#define MAX_UNKNOWNS 200 + + IdList entity; + IdList param; + IdList eq; + + // In general, the tag indicates the subsys that a variable/equation + // has been assigned to; these are exceptions. + static const int ASSUMED = 10000; + static const int SUBSTITUTED = 10001; + + // The Jacobian matrix + struct { + hEquation eq[MAX_UNKNOWNS]; + struct { + Expr *sym[MAX_UNKNOWNS]; + double num[MAX_UNKNOWNS]; + } B; + + hParam param[MAX_UNKNOWNS]; + bool bound[MAX_UNKNOWNS]; + + Expr *sym[MAX_UNKNOWNS][MAX_UNKNOWNS]; + double num[MAX_UNKNOWNS][MAX_UNKNOWNS]; + + double X[MAX_UNKNOWNS]; + + int m, n; + } J; + + bool Tol(double v); + void GaussJordan(void); + bool SolveLinearSystem(void); + + void WriteJacobian(int eqTag, int paramTag); + void EvalJacobian(void); + + bool NewtonSolve(int tag); + bool Solve(void); +}; + class SolveSpace { public: @@ -93,6 +136,7 @@ public: inline Request *GetRequest(hRequest h) { return request.FindById(h); } inline Entity *GetEntity (hEntity h) { return entity. FindById(h); } inline Param *GetParam (hParam h) { return param. FindById(h); } + inline Group *GetGroup (hGroup h) { return group. FindById(h); } hGroup activeGroup; @@ -102,11 +146,17 @@ public: void ForceReferences(void); void Init(void); + + bool SolveGroup(hGroup hg); + bool SolveWorker(void); void Solve(void); static void MenuFile(int id); bool SaveToFile(char *filename); bool LoadFromFile(char *filename); + + // The system to be solved. + System sys; }; extern SolveSpace SS; diff --git a/system.cpp b/system.cpp new file mode 100644 index 0000000..e49a765 --- /dev/null +++ b/system.cpp @@ -0,0 +1,244 @@ +#include "solvespace.h" + +void System::WriteJacobian(int eqTag, int paramTag) { + int a, i, j; + + j = 0; + for(a = 0; a < param.n; a++) { + Param *p = &(param.elem[a]); + if(p->tag != paramTag) continue; + J.param[j] = p->h; + j++; + } + J.n = j; + + i = 0; + for(a = 0; a < eq.n; a++) { + Equation *e = &(eq.elem[a]); + if(e->tag != eqTag) continue; + + J.eq[i] = eq.elem[i].h; + J.B.sym[i] = eq.elem[i].e; + for(j = 0; j < J.n; j++) { + J.sym[i][j] = e->e->PartialWrt(J.param[j]); + } + i++; + } + J.m = i; +} + +void System::EvalJacobian(void) { + int i, j; + for(i = 0; i < J.m; i++) { + for(j = 0; j < J.n; j++) { + J.num[i][j] = (J.sym[i][j])->Eval(); + } + } +} + +bool System::Tol(double v) { + return (fabs(v) < 0.01); +} + +void System::GaussJordan(void) { + int i, j; + + for(j = 0; j < J.n; j++) { + J.bound[j] = false; + } + + // Now eliminate. + i = 0; + for(j = 0; j < J.n; j++) { + // First, seek a pivot in our column. + int ip, imax; + double max = 0; + for(ip = i; ip < J.m; ip++) { + double v = fabs(J.num[ip][j]); + if(v > max) { + imax = ip; + max = v; + } + } + if(!Tol(max)) { + // There's a usable pivot in this column. Swap it in: + int js; + for(js = j; js < J.n; js++) { + double temp; + temp = J.num[imax][js]; + J.num[imax][js] = J.num[i][js]; + J.num[i][js] = temp; + } + + // Get a 1 as the leading entry in the row we're working on. + double v = J.num[i][j]; + for(js = 0; js < J.n; js++) { + J.num[i][js] /= v; + } + + // Eliminate this column from rows except this one. + int is; + for(is = 0; is < J.m; is++) { + if(is == i) continue; + + // We're trying to drive J.num[is][j] to zero. We know + // that J.num[i][j] is 1, so we want to subtract off + // J.num[is][j] times our present row. + double v = J.num[is][j]; + for(js = 0; js < J.n; js++) { + J.num[is][js] -= v*J.num[i][js]; + } + J.num[is][j] = 0; + } + + // And mark this as a bound variable. + J.bound[j] = true; + + // Move on to the next row, since we just used this one to + // eliminate from column j. + i++; + if(i >= J.m) break; + } + } +} + +bool System::SolveLinearSystem(void) { + if(J.m != J.n) oops(); + // Gaussian elimination, with partial pivoting. It's an error if the + // matrix is singular, because that means two constraints are + // equivalent. + int i, j, ip, jp, imax; + double max, temp; + + for(i = 0; i < J.m; i++) { + // We are trying eliminate the term in column i, for rows i+1 and + // greater. First, find a pivot (between rows i and N-1). + max = 0; + for(ip = i; ip < J.m; ip++) { + if(fabs(J.num[ip][i]) > max) { + imax = ip; + max = fabs(J.num[ip][i]); + } + } + if(fabs(max) < 1e-12) return false; + + // Swap row imax with row i + for(jp = 0; jp < J.n; jp++) { + temp = J.num[i][jp]; + J.num[i][jp] = J.num[imax][jp]; + J.num[imax][jp] = temp; + } + temp = J.B.num[i]; + J.B.num[i] = J.B.num[imax]; + J.B.num[imax] = temp; + + // For rows i+1 and greater, eliminate the term in column i. + for(ip = i+1; ip < J.m; ip++) { + temp = J.num[ip][i]/J.num[i][i]; + + for(jp = 0; jp < J.n; jp++) { + J.num[ip][jp] -= temp*(J.num[i][jp]); + } + J.B.num[ip] -= temp*J.B.num[i]; + } + } + + // We've put the matrix in upper triangular form, so at this point we + // can solve by back-substitution. + for(i = J.m - 1; i >= 0; i--) { + if(fabs(J.num[i][i]) < 1e-10) return false; + + temp = J.B.num[i]; + for(j = J.n - 1; j > i; j--) { + temp -= J.X[j]*J.num[i][j]; + } + J.X[i] = temp / J.num[i][i]; + } + + return true; +} + +bool System::NewtonSolve(int tag) { + WriteJacobian(tag, tag); + if(J.m != J.n) oops(); + + int iter = 0; + bool converged = false; + int i; + do { + // Evaluate the functions numerically + for(i = 0; i < J.m; i++) { + J.B.num[i] = (J.B.sym[i])->Eval(); + dbp("J.B.num[%d] = %.3f", i, J.B.num[i]); + dbp("J.B.sym[%d] = %s", i, (J.B.sym[i])->Print()); + } + // And likewise for the Jacobian + EvalJacobian(); + + if(!SolveLinearSystem()) break; + + // Take the Newton step; + // J(x_n) (x_{n+1} - x_n) = 0 - F(x_n) + for(i = 0; i < J.m; i++) { + dbp("J.X[%d] = %.3f", i, J.X[i]); + dbp("modifying param %08x, now %.3f", J.param[i], + param.FindById(J.param[i])->val); + (param.FindById(J.param[i]))->val -= J.X[i]; + // XXX do this properly + SS.GetParam(J.param[i])->val = (param.FindById(J.param[i]))->val; + } + + // XXX re-evaluate functions before checking convergence + converged = true; + for(i = 0; i < J.m; i++) { + if(!Tol(J.B.num[i])) { + converged = false; + break; + } + } + } while(iter++ < 50 && !converged); + + if(converged) { + return true; + } else { + return false; + } +} + +bool System::Solve(void) { + int i, j; + dbp("%d equations", eq.n); + for(i = 0; i < eq.n; i++) { + dbp(" %s = 0", eq.elem[i].e->Print()); + } + + param.ClearTags(); + eq.ClearTags(); + + WriteJacobian(0, 0); + EvalJacobian(); + + for(i = 0; i < J.m; i++) { + for(j = 0; j < J.n; j++) { + dbp("J[%d][%d] = %.3f", i, j, J.num[i][j]); + } + } + + GaussJordan(); + + dbp("bound states:"); + for(j = 0; j < J.n; j++) { + dbp(" param %08x: %d", J.param[j], J.bound[j]); + } + + // Fix any still-free variables wherever they are now. + for(j = 0; j < J.n; j++) { + if(J.bound[j]) continue; + param.FindById(J.param[j])->tag = ASSUMED; + } + + NewtonSolve(0); + + return true; +} + diff --git a/ui.h b/ui.h index 53d37f6..345c359 100644 --- a/ui.h +++ b/ui.h @@ -96,6 +96,7 @@ public: MNU_LINE_SEGMENT, // Constrain MNU_DISTANCE_DIA, + MNU_SOLVE_NOW, } MenuId; typedef void MenuHandler(int id); typedef struct {