Add symmetric and at midpoint constraints. The symmetry constraint

turned out straightforward, in great part because the planes are
workplanes (6 DOF, represented by a unit quaternion and a point),
and therefore make it easy to get a vector in the plane, as well as
a normal.

And on that subject, replace the previous hack for parallel vector
constraints with a better hack: pivot on the initial numerical
guess, to choose which components of the cross product to drive to
zero. Ugly, but I think that will be as robust as I can get.

[git-p4: depot-paths = "//depot/solvespace/": change = 1699]

constraint.cpp

@@ -91,6 +91,38 @@ void Constraint::MenuConstrain(int id) {
             AddConstraint(&c);
             break;
 
+        case GraphicsWindow::MNU_AT_MIDPOINT:
+            if(gs.lineSegments == 1 && gs.points == 1 && gs.n == 2) {
+                c.type = AT_MIDPOINT;
+                c.entityA = gs.entity[0];
+                c.ptA = gs.point[0];
+            } else {
+                Error("Bad selection for at midpoint constraint.");
+                return;
+            }
+            AddConstraint(&c);
+            break;
+
+        case GraphicsWindow::MNU_SYMMETRIC:
+            if(gs.points == 2 && gs.planes == 1 && gs.n == 3) {
+                c.type = SYMMETRIC;
+                c.entityA = gs.entity[0];
+                c.ptA = gs.point[0];
+                c.ptB = gs.point[1];
+            } else if(gs.lineSegments == 1 && gs.planes == 1 && gs.n == 2) {
+                c.type = SYMMETRIC;
+                int i = SS.GetEntity(gs.entity[0])->HasPlane() ? 1 : 0;
+                Entity *line = SS.GetEntity(gs.entity[i]);
+                c.entityA = gs.entity[1-i];
+                c.ptA = line->point[0];
+                c.ptB = line->point[1];
+            } else {
+                Error("Bad selection for symmetric constraint.");
+                return;
+            }
+            AddConstraint(&c);
+            break;
+
         case GraphicsWindow::MNU_VERTICAL:
         case GraphicsWindow::MNU_HORIZONTAL: {
             hEntity ha, hb;
@@ -139,6 +171,30 @@ void Constraint::MenuConstrain(int id) {
     InvalidateGraphics();
 }
 
+Expr *Constraint::VectorsParallel(int eq, ExprVector a, ExprVector b) {
+    ExprVector r = a.Cross(b);
+    // Hairy ball theorem screws me here. There's no clean solution that I
+    // know, so let's pivot on the initial numerical guess.
+    double mx = fabs((a.x)->Eval()) + fabs((b.x)->Eval());
+    double my = fabs((a.y)->Eval()) + fabs((b.y)->Eval());
+    double mz = fabs((a.z)->Eval()) + fabs((b.z)->Eval());
+    // The basis vector in which the vectors have the LEAST energy is the
+    // one that we should look at most (e.g. if both vectors lie in the xy
+    // plane, then the z component of the cross product is most important).
+    // So find the strongest component of a and b, and that's the component
+    // of the cross product to ignore.
+    double m = max(mx, max(my, mz));
+    Expr *e0, *e1;
+         if(m == mx) { e0 = r.y; e1 = r.z; }
+    else if(m == my) { e0 = r.z; e1 = r.x; }
+    else if(m == mz) { e0 = r.x; e1 = r.y; }
+    else oops();
+
+    if(eq == 0) return e0;
+    if(eq == 1) return e1;
+    oops();
+}
+
 Expr *Constraint::PointLineDistance(hEntity wrkpl, hEntity hpt, hEntity hln) {
     Entity *ln = SS.GetEntity(hln);
     Entity *a = SS.GetEntity(ln->point[0]);
@@ -175,6 +231,13 @@ Expr *Constraint::PointLineDistance(hEntity wrkpl, hEntity hpt, hEntity hln) {
     }
 }
 
+Expr *Constraint::PointPlaneDistance(ExprVector p, hEntity hpl) {
+    ExprVector n;
+    Expr *d;
+    SS.GetEntity(hpl)->PlaneGetExprs(&n, &d);
+    return (p.Dot(n))->Minus(d);
+}
+
 Expr *Constraint::Distance(hEntity wrkpl, hEntity hpa, hEntity hpb) {
     Entity *pa = SS.GetEntity(hpa);
     Entity *pb = SS.GetEntity(hpb);
@@ -261,15 +324,11 @@ void Constraint::Generate(IdList<Equation,hEquation> *l) {
             break;
         }
 
-        case PT_IN_PLANE: {
+        case PT_IN_PLANE:
             // This one works the same, whether projected or not.
-            ExprVector p, n;
+            AddEq(l, PointPlaneDistance(
-            Expr *d;
+                        SS.GetEntity(ptA)->PointGetExprs(), entityA), 0);
-            p = SS.GetEntity(ptA)->PointGetExprs();
-            SS.GetEntity(entityA)->PlaneGetExprs(&n, &d);
-            AddEq(l, (p.Dot(n))->Minus(d), 0);
             break;
-        }
 
         case PT_ON_LINE:
             if(workplane.v == Entity::FREE_IN_3D.v) {
@@ -282,19 +341,96 @@ void Constraint::Generate(IdList<Equation,hEquation> *l) {
                 ExprVector ea = a->PointGetExprs();
                 ExprVector eb = b->PointGetExprs();
                 ExprVector eab = ea.Minus(eb);
-                ExprVector r = eab.Cross(ea.Minus(ep));
+                ExprVector eap = ea.Minus(ep);
 
-                // When the constraint is satisfied, our vector r is zero;
+                AddEq(l, VectorsParallel(0, eab, eap), 0);
-                // but that's three numbers, and the constraint hits only
+                AddEq(l, VectorsParallel(1, eab, eap), 1);
-                // two degrees of freedom. This seems to be an acceptable
-                // choice of equations, though it's arbitrary.
-                AddEq(l, (r.x)->Square()->Plus((r.y)->Square()), 0);
-                AddEq(l, (r.y)->Square()->Plus((r.z)->Square()), 1);
             } else {
                 AddEq(l, PointLineDistance(workplane, ptA, entityA), 0);
             }
             break;
 
+        case AT_MIDPOINT:
+            if(workplane.v == Entity::FREE_IN_3D.v) {
+                Entity *ln = SS.GetEntity(entityA);
+                ExprVector a = SS.GetEntity(ln->point[0])->PointGetExprs();
+                ExprVector b = SS.GetEntity(ln->point[1])->PointGetExprs();
+                ExprVector m = (a.Plus(b)).ScaledBy(Expr::FromConstant(0.5));
+
+                ExprVector p = SS.GetEntity(ptA)->PointGetExprs();
+                AddEq(l, (m.x)->Minus(p.x), 0);
+                AddEq(l, (m.y)->Minus(p.y), 1);
+                AddEq(l, (m.z)->Minus(p.z), 2);
+            } else {
+                Entity *ln = SS.GetEntity(entityA);
+                Entity *a = SS.GetEntity(ln->point[0]);
+                Entity *b = SS.GetEntity(ln->point[1]);
+                
+                Expr *au, *av, *bu, *bv;
+                a->PointGetExprsInWorkplane(workplane, &au, &av);
+                b->PointGetExprsInWorkplane(workplane, &bu, &bv);
+                Expr *mu = Expr::FromConstant(0.5)->Times(au->Plus(bu));
+                Expr *mv = Expr::FromConstant(0.5)->Times(av->Plus(bv));
+
+                Entity *p = SS.GetEntity(ptA);
+                Expr *pu, *pv;
+                p->PointGetExprsInWorkplane(workplane, &pu, &pv);
+                AddEq(l, pu->Minus(mu), 0);
+                AddEq(l, pv->Minus(mv), 1);
+            }
+            break;
+
+        case SYMMETRIC:
+            if(workplane.v == Entity::FREE_IN_3D.v) {
+                Entity *plane = SS.GetEntity(entityA);
+                Entity *ea = SS.GetEntity(ptA);
+                Entity *eb = SS.GetEntity(ptB);
+                ExprVector a = ea->PointGetExprs();
+                ExprVector b = eb->PointGetExprs();
+
+                // The midpoint of the line connecting the symmetric points
+                // lies on the plane of the symmetry.
+                ExprVector m = (a.Plus(b)).ScaledBy(Expr::FromConstant(0.5));
+                AddEq(l, PointPlaneDistance(m, plane->h), 0);
+
+                // And projected into the plane of symmetry, the points are
+                // coincident.
+                Expr *au, *av, *bu, *bv;
+                ea->PointGetExprsInWorkplane(plane->h, &au, &av);
+                eb->PointGetExprsInWorkplane(plane->h, &bu, &bv);
+                AddEq(l, au->Minus(bu), 0);
+                AddEq(l, av->Minus(bv), 1);
+            } else {
+                Entity *plane = SS.GetEntity(entityA);
+                Entity *a = SS.GetEntity(ptA);
+                Entity *b = SS.GetEntity(ptB);
+
+                Expr *au, *av, *bu, *bv;
+                a->PointGetExprsInWorkplane(workplane, &au, &av);
+                b->PointGetExprsInWorkplane(workplane, &bu, &bv);
+                Expr *mu = Expr::FromConstant(0.5)->Times(au->Plus(bu));
+                Expr *mv = Expr::FromConstant(0.5)->Times(av->Plus(bv));
+
+                ExprVector u, v, o;
+                Entity *w = SS.GetEntity(workplane);
+                w->WorkplaneGetBasisExprs(&u, &v);
+                o = w->WorkplaneGetOffsetExprs();
+                ExprVector m = (u.ScaledBy(mu)).Plus(v.ScaledBy(mv)).Plus(o);
+                AddEq(l, PointPlaneDistance(m ,plane->h), 0);
+
+                // Construct a vector within the workplane that is normal
+                // to the symmetry pane's normal (i.e., that lies in the
+                // plane of symmetry). The line connecting the points is
+                // perpendicular to that constructed vector.
+                ExprVector pa = a->PointGetExprs();
+                ExprVector pb = b->PointGetExprs();
+                ExprVector n;
+                Expr *d;
+                plane->PlaneGetExprs(&n, &d);
+                AddEq(l, (n.Cross(u.Cross(v))).Dot(pa.Minus(pb)), 1);
+            }
+            break;
+
         case HORIZONTAL:
         case VERTICAL: {
             hEntity ha, hb;

drawconstraint.cpp

@@ -117,7 +117,6 @@ void Constraint::DrawOrGetDistance(Vector *labelPos) {
             break;
         }
 
-
         case EQUAL_LENGTH_LINES: {
             for(int i = 0; i < 2; i++) {
                 Entity *e = SS.GetEntity(i == 0 ? entityA : entityB);
@@ -132,19 +131,50 @@ void Constraint::DrawOrGetDistance(Vector *labelPos) {
             break;
         }
 
+        case SYMMETRIC: {
+            Vector a = SS.GetEntity(ptA)->PointGetCoords();
+            Vector b = SS.GetEntity(ptB)->PointGetCoords();
+            Vector n = SS.GetEntity(entityA)->WorkplaneGetNormalVector();
+            for(int i = 0; i < 2; i++) {
+                Vector tail = (i == 0) ? a : b;
+                Vector d = (i == 0) ? b : a;
+                d = d.Minus(tail);
+                // Project the direction in which the arrow is drawn normal
+                // to the symmetry plane; for projected symmetry constraints,
+                // they might not be in the same direction, even when the
+                // constraint is fully solved.
+                d = n.ScaledBy(d.Dot(n));
+                d = d.WithMagnitude(20/SS.GW.scale);
+                Vector tip = tail.Plus(d);
+
+                LineDrawOrGetDistance(tail, tip);
+                d = d.WithMagnitude(9/SS.GW.scale);
+                LineDrawOrGetDistance(tip, tip.Minus(d.RotatedAbout(gn,  0.6)));
+                LineDrawOrGetDistance(tip, tip.Minus(d.RotatedAbout(gn, -0.6)));
+            }
+            break;
+        }
+
+        case AT_MIDPOINT:
         case HORIZONTAL:
         case VERTICAL:
             if(entityA.v) {
+                // For "at midpoint", this branch is always taken.
                 Entity *e = SS.GetEntity(entityA);
                 Vector a = SS.GetEntity(e->point[0])->PointGetCoords();
                 Vector b = SS.GetEntity(e->point[1])->PointGetCoords();
                 Vector m = (a.ScaledBy(0.5)).Plus(b.ScaledBy(0.5));
+                Vector offset = (a.Minus(b)).Cross(gn);
+                offset = offset.WithMagnitude(13/SS.GW.scale);
 
                 if(dogd.drawing) {
                     glPushMatrix();
-                        glxTranslatev(m);
+                        glxTranslatev(m.Plus(offset));
                         glxOntoWorkplane(gr, gu);
-                        glxWriteText(type == HORIZONTAL ? "H" : "V");
+                        glxWriteTextRefCenter(
+                            (type == HORIZONTAL)  ? "H" : (
+                            (type == VERTICAL)    ? "V" : (
+                            (type == AT_MIDPOINT) ? "M" : NULL)));
                     glPopMatrix();
                 } else {
                     Point2d ref = SS.GW.ProjectPoint(m);

glhelper.cpp

@@ -5,9 +5,35 @@
 
 static bool ColorLocked;
 
+#define FONT_SCALE (0.5)
+static int StrWidth(char *str) {
+    int w = 0;
+    for(; *str; str++) {
+        int c = *str;
+        if(c < 32 || c > 126) c = 32;
+        c -= 32;
+
+        w += Font[c].width;
+    }
+    return w;
+}
+void glxWriteTextRefCenter(char *str)
+{
+    double scale = FONT_SCALE/SS.GW.scale;
+    // The characters have height ~21, as they appear in the table.
+    double fh = (21.0)*scale;
+    double fw = StrWidth(str)*scale;
+    glPushMatrix();
+        glTranslated(-fw/2, -fh/2, 0);
+        // Undo the (+5, +5) offset that glxWriteText applies.
+        glTranslated(-5*scale, -5*scale, 0);
+        glxWriteText(str);
+    glPopMatrix();
+}
+
 void glxWriteText(char *str)
 {
-    double scale = 0.5/SS.GW.scale;
+    double scale = FONT_SCALE/SS.GW.scale;
     int xo = 5;
     int yo = 5;
     

graphicswin.cpp

@@ -78,8 +78,8 @@ const GraphicsWindow::MenuEntry GraphicsWindow::menu[] = {
 { 1, NULL,                                  0,                          NULL  },
 { 1, "&On Point / Curve / Plane\tShift+O",  MNU_ON_ENTITY,      'O'|S,  mCon  },
 { 1, "E&qual Length / Radius\tShift+Q",     MNU_EQUAL,          'Q'|S,  mCon  },
-{ 1, "At &Midpoint\tShift+M",               0,                  'M'|S,  NULL  },
+{ 1, "At &Midpoint\tShift+M",               MNU_AT_MIDPOINT,    'M'|S,  mCon  },
-{ 1, "S&ymmetric\tShift+Y",                 0,                  'Y'|S,  NULL  },
+{ 1, "S&ymmetric\tShift+Y",                 MNU_SYMMETRIC,      'Y'|S,  mCon  },
 { 1, NULL,                                  0,                          NULL  },
 { 1, "Sym&bolic Equation\tShift+B",         0,                  'B'|S,  NULL  },
 { 1, NULL,                                  0,                          NULL  },

sketch.h

@@ -262,6 +262,8 @@ public:
     static const int PT_IN_PLANE        = 40;
     static const int PT_ON_LINE         = 41;
     static const int EQUAL_LENGTH_LINES = 50;
+    static const int SYMMETRIC          = 60;
+    static const int AT_MIDPOINT        = 70;
 
     static const int HORIZONTAL         = 80;
     static const int VERTICAL           = 81;
@@ -313,6 +315,8 @@ public:
     void AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index);
     static Expr *Distance(hEntity workplane, hEntity pa, hEntity pb);
     static Expr *PointLineDistance(hEntity workplane, hEntity pt, hEntity ln);
+    static Expr *PointPlaneDistance(ExprVector p, hEntity plane);
+    static Expr *VectorsParallel(int eq, ExprVector a, ExprVector b);
 
     static void ConstrainCoincident(hEntity ptA, hEntity ptB);
 };

solvespace.h

@@ -67,6 +67,7 @@ void MemFree(void *p);
 void glxVertex3v(Vector u);
 void glxFillPolygon(SPolygon *p);
 void glxWriteText(char *str);
+void glxWriteTextRefCenter(char *str);
 void glxTranslatev(Vector u);
 void glxOntoWorkplane(Vector u, Vector v);
 void glxLockColorTo(double r, double g, double b);

system.cpp

@@ -211,6 +211,7 @@ bool System::NewtonSolve(int tag) {
     if(converged) { 
         return true;
     } else {
+        dbp("no convergence");
         return false;
     }
 }

ui.h

@@ -111,6 +111,8 @@ public:
         MNU_DISTANCE_DIA,
         MNU_EQUAL,
         MNU_ON_ENTITY,
+        MNU_SYMMETRIC,
+        MNU_AT_MIDPOINT,
         MNU_HORIZONTAL,
         MNU_VERTICAL,
         MNU_SOLVE_AUTO,