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

@@ -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

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<Equation,hEquation> *l, Expr *expr, int index) {
+    Equation eq;
+    eq.e = expr;
+    eq.h = h.equation(index);
+    l->Add(&eq);
+}
+
+void Constraint::Generate(IdList<Equation,hEquation> *l) {
+    switch(type) {
+        case PT_PT_DISTANCE:
+            AddEq(l, Distance(ptA, ptB)->Minus(exprA), 0);
+            break;
+
+        default: oops();
+    }
+}

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);

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  },

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<EntityId,EntityMap> remap;
+    hEntity Remap(hEntity in, int copyNumber);
+
     hParam AddParam(IdList<Param,hParam> *param, hParam hp);
     void Generate(IdList<Entity,hEntity> *entity, IdList<Param,hParam> *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<Equation,hEquation> *l);
+    // Some helpers when generating symbolic constraint equations
+    void AddEq(IdList<Equation,hEquation> *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

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) {

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,hEntity>          entity;
+    IdList<Param,hParam>            param;
+    IdList<Equation,hEquation>      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;

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;
+}
+

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 {