Add equal-angle constraints. These are implemented by brute force,

as the difference between the cosines of the two angles. All of the
angle stuff generates huge expressions (Expr *), but doesn't seem
noticeably slow.

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

constraint.cpp

@@ -35,6 +35,7 @@ char *Constraint::DescriptionString(void) {
         case CUBIC_LINE_TANGENT:s = "cubic-line-tangent"; break;
         case PERPENDICULAR:     s = "perpendicular"; break;
         case EQUAL_RADIUS:      s = "eq-radius"; break;
+        case EQUAL_ANGLE:       s = "eq-angle"; break;
         case COMMENT:           s = "comment"; break;
         default:                s = "???"; break;
     }
@@ -199,6 +200,18 @@ void Constraint::MenuConstrain(int id) {
                 c.entityA = gs.entity[0];
                 c.entityB = gs.entity[1];
                 c.ptA = gs.point[0];
+            } else if(gs.vectors == 4 && gs.n == 4) {
+                c.type = EQUAL_ANGLE;
+                c.entityA = gs.vector[0];
+                c.entityB = gs.vector[1];
+                c.entityC = gs.vector[2];
+                c.entityD = gs.vector[3];
+            } else if(gs.vectors == 3 && gs.n == 3) {
+                c.type = EQUAL_ANGLE;
+                c.entityA = gs.vector[0];
+                c.entityB = gs.vector[1];
+                c.entityC = gs.vector[1];
+                c.entityD = gs.vector[2];
             } else if(gs.circlesOrArcs == 2 && gs.n == 2) {
                 c.type = EQUAL_RADIUS;
                 c.entityA = gs.entity[0];
@@ -213,6 +226,10 @@ void Constraint::MenuConstrain(int id) {
                               "(equal point-line distances)\r\n"
                       "    * a line segment, and a point and line segment "
                               "(point-line distance equals length)\r\n"
+                      "    * four line segments or normals "
+                              "(equal angle between A,B and C,D)\r\n"
+                      "    * three line segments or normals "
+                              "(equal angle between A,B and B,C)\r\n"
                       "    * two circles or arcs (equal radius)\r\n");
                 return;
             }
@@ -428,10 +445,18 @@ void Constraint::MenuConstrain(int id) {
             if(gs.constraints == 1 && gs.n == 0) {
                 Constraint *c = SS.GetConstraint(gs.constraint[0]);
                 if(c->type == ANGLE) {
+                    SS.UndoRemember();
                     c->other = !(c->other);
                     c->ModifyToSatisfy();
                     break;
                 }
+                if(c->type == EQUAL_ANGLE) {
+                    SS.UndoRemember();
+                    c->other = !(c->other);
+                    SS.MarkGroupDirty(c->group);
+                    SS.later.generateAll = true;
+                    break;
+                }
             }
             Error("Must select an angle constraint.");
             return;
@@ -663,6 +688,29 @@ Expr *Constraint::Distance(hEntity wrkpl, hEntity hpa, hEntity hpb) {
     }
 }
 
+//-----------------------------------------------------------------------------
+// Return the cosine of the angle between two vectors. If a workplane is
+// specified, then it's the cosine of their projections into that workplane.
+//-----------------------------------------------------------------------------
+Expr *Constraint::DirectionCosine(hEntity wrkpl, ExprVector ae, ExprVector be) {
+    if(wrkpl.v == Entity::FREE_IN_3D.v) {
+        Expr *mags = (ae.Magnitude())->Times(be.Magnitude());
+        return (ae.Dot(be))->Div(mags);
+    } else {
+        Entity *w = SS.GetEntity(wrkpl);
+        ExprVector u = w->Normal()->NormalExprsU();
+        ExprVector v = w->Normal()->NormalExprsV();
+        Expr *ua = u.Dot(ae);
+        Expr *va = v.Dot(ae);
+        Expr *ub = u.Dot(be);
+        Expr *vb = v.Dot(be);
+        Expr *maga = (ua->Square()->Plus(va->Square()))->Sqrt();
+        Expr *magb = (ub->Square()->Plus(vb->Square()))->Sqrt();
+        Expr *dot = (ua->Times(ub))->Plus(va->Times(vb));
+        return dot->Div(maga->Times(magb));
+    }
+}
+
 ExprVector Constraint::PointInThreeSpace(hEntity workplane, Expr *u, Expr *v) {
     Entity *w = SS.GetEntity(workplane);
 
@@ -1076,23 +1124,8 @@ void Constraint::GenerateReal(IdList<Equation,hEquation> *l) {
             ExprVector ae = a->VectorGetExprs();
             ExprVector be = b->VectorGetExprs();
             if(other) ae = ae.ScaledBy(Expr::From(-1));
-            Expr *c;
+            Expr *c = DirectionCosine(workplane, ae, be);
-            if(workplane.v == Entity::FREE_IN_3D.v) {
+
-                Expr *mags = (ae.Magnitude())->Times(be.Magnitude());
-                c = (ae.Dot(be))->Div(mags);
-            } else {
-                Entity *w = SS.GetEntity(workplane);
-                ExprVector u = w->Normal()->NormalExprsU();
-                ExprVector v = w->Normal()->NormalExprsV();
-                Expr *ua = u.Dot(ae);
-                Expr *va = v.Dot(ae);
-                Expr *ub = u.Dot(be);
-                Expr *vb = v.Dot(be);
-                Expr *maga = (ua->Square()->Plus(va->Square()))->Sqrt();
-                Expr *magb = (ub->Square()->Plus(vb->Square()))->Sqrt();
-                Expr *dot = (ua->Times(ub))->Plus(va->Times(vb));
-                c = dot->Div(maga->Times(magb));
-            }
             if(type == ANGLE) {
                 // The direction cosine is equal to the cosine of the
                 // specified angle
@@ -1106,6 +1139,25 @@ void Constraint::GenerateReal(IdList<Equation,hEquation> *l) {
             break;
         }
 
+        case EQUAL_ANGLE: {
+            Entity *a = SS.GetEntity(entityA);
+            Entity *b = SS.GetEntity(entityB);
+            Entity *c = SS.GetEntity(entityC);
+            Entity *d = SS.GetEntity(entityD);
+            ExprVector ae = a->VectorGetExprs();
+            ExprVector be = b->VectorGetExprs();
+            ExprVector ce = c->VectorGetExprs();
+            ExprVector de = d->VectorGetExprs();
+
+            if(other) ae = ae.ScaledBy(Expr::From(-1));
+
+            Expr *cab = DirectionCosine(workplane, ae, be);
+            Expr *ccd = DirectionCosine(workplane, ce, de);
+
+            AddEq(l, cab->Minus(ccd), 0);
+            break;
+        }
+
         case ARC_LINE_TANGENT: {
             Entity *arc  = SS.GetEntity(entityA);
             Entity *line = SS.GetEntity(entityB);

drawconstraint.cpp

@@ -104,6 +104,75 @@ void Constraint::DoEqualLenTicks(Vector a, Vector b, Vector gn) {
     LineDrawOrGetDistance(m.Minus(n), m.Plus(n));
 }
 
+void Constraint::DoArcForAngle(Vector a0, Vector da, Vector b0, Vector db,
+                                   Vector offset, Vector *ref)
+{
+    Vector gr = SS.GW.projRight.ScaledBy(1/SS.GW.scale);
+    Vector gu = SS.GW.projUp.ScaledBy(1/SS.GW.scale);
+
+    if(workplane.v != Entity::FREE_IN_3D.v) {
+        a0 = a0.ProjectInto(workplane);
+        b0 = b0.ProjectInto(workplane);
+        da = da.ProjectVectorInto(workplane);
+        db = db.ProjectVectorInto(workplane);
+    }
+
+    // Make an orthogonal coordinate system from those directions
+    Vector dn = da.Cross(db); // normal to both
+    Vector dna = dn.Cross(da); // normal to da
+    Vector dnb = dn.Cross(db); // normal to db
+    // At the intersection of the lines
+    //    a0 + pa*da = b0 + pb*db (where pa, pb are scalar params)
+    // So dot this equation against dna and dnb to get two equations
+    // to solve for da and db
+    double pb =  ((a0.Minus(b0)).Dot(dna))/(db.Dot(dna));
+    double pa = -((a0.Minus(b0)).Dot(dnb))/(da.Dot(dnb));
+
+    Vector pi = a0.Plus(da.ScaledBy(pa));
+    if(pi.Equals(b0.Plus(db.ScaledBy(pb)))) {
+        *ref = pi.Plus(offset);
+        // We draw in a coordinate system centered at pi, with
+        // basis vectors da and dna.
+        da = da.WithMagnitude(1); dna = dna.WithMagnitude(1);
+        Vector rm = (*ref).Minus(pi);
+        double rda = rm.Dot(da), rdna = rm.Dot(dna);
+        double r = sqrt(rda*rda + rdna*rdna);
+        double c = (da.Dot(db))/(da.Magnitude()*db.Magnitude());
+        double thetaf = acos(c);
+
+        Vector m = da.ScaledBy(cos(thetaf/2)).Plus(
+                   dna.ScaledBy(sin(thetaf/2)));
+        if(m.Dot(rm) < 0) {
+            da = da.ScaledBy(-1); dna = dna.ScaledBy(-1);
+        }
+
+        Vector prev = da.ScaledBy(r).Plus(pi);
+        int i, n = 30;
+        for(i = 0; i <= n; i++) {
+            double theta = (i*thetaf)/n;
+            Vector p = da. ScaledBy(r*cos(theta)).Plus(
+                       dna.ScaledBy(r*sin(theta))).Plus(pi);
+            LineDrawOrGetDistance(prev, p);
+            prev = p;
+        }
+
+        double tl = atan2(rm.Dot(gu), rm.Dot(gr));
+        double adj = EllipticalInterpolation(
+            glxStrWidth(Label())/2, glxStrHeight()/2, tl);
+        *ref = (*ref).Plus(rm.WithMagnitude(adj + 3/SS.GW.scale));
+    } else {
+        // The lines are skew; no wonderful way to illustrate that.
+        *ref = a0.Plus(b0);
+        *ref = (*ref).ScaledBy(0.5).Plus(disp.offset);
+        glPushMatrix();
+            gu = gu.WithMagnitude(1);
+            glxTranslatev((*ref).Plus(gu.ScaledBy(-1.5*glxStrHeight())));
+            glxOntoWorkplane(gr, gu);
+            glxWriteTextRefCenter("angle between skew lines");
+        glPopMatrix();
+    }
+}
+
 void Constraint::DrawOrGetDistance(Vector *labelPos) {
     if(!SS.GW.showConstraints) return;
     Group *g = SS.GetGroup(group);
@@ -297,6 +366,32 @@ void Constraint::DrawOrGetDistance(Vector *labelPos) {
             break;
         }
 
+        case EQUAL_ANGLE: {
+            Vector ref;
+            Entity *a = SS.GetEntity(entityA);
+            Entity *b = SS.GetEntity(entityB);
+            Entity *c = SS.GetEntity(entityC);
+            Entity *d = SS.GetEntity(entityD);
+
+            Vector a0 = a->VectorGetRefPoint();
+            Vector b0 = b->VectorGetRefPoint();
+            Vector c0 = c->VectorGetRefPoint();
+            Vector d0 = d->VectorGetRefPoint();
+            Vector da = a->VectorGetNum();
+            Vector db = b->VectorGetNum();
+            Vector dc = c->VectorGetNum();
+            Vector dd = d->VectorGetNum();
+
+            if(other) da = da.ScaledBy(-1);
+
+            DoArcForAngle(a0, da, b0, db, 
+                da.WithMagnitude(40/SS.GW.scale), &ref);
+            DoArcForAngle(c0, dc, d0, dd, 
+                dc.WithMagnitude(40/SS.GW.scale), &ref);
+
+            break;
+        }
+
         case ANGLE: {
             Entity *a = SS.GetEntity(entityA);
             Entity *b = SS.GetEntity(entityB);
@@ -307,69 +402,8 @@ void Constraint::DrawOrGetDistance(Vector *labelPos) {
             Vector db = b->VectorGetNum();
             if(other) da = da.ScaledBy(-1);
 
-            if(workplane.v != Entity::FREE_IN_3D.v) {
-                a0 = a0.ProjectInto(workplane);
-                b0 = b0.ProjectInto(workplane);
-                da = da.ProjectVectorInto(workplane);
-                db = db.ProjectVectorInto(workplane);
-            }
-
-            // Make an orthogonal coordinate system from those directions
-            Vector dn = da.Cross(db); // normal to both
-            Vector dna = dn.Cross(da); // normal to da
-            Vector dnb = dn.Cross(db); // normal to db
-            // At the intersection of the lines
-            //    a0 + pa*da = b0 + pb*db (where pa, pb are scalar params)
-            // So dot this equation against dna and dnb to get two equations
-            // to solve for da and db
-            double pb =  ((a0.Minus(b0)).Dot(dna))/(db.Dot(dna));
-            double pa = -((a0.Minus(b0)).Dot(dnb))/(da.Dot(dnb));
-
-            Vector pi = a0.Plus(da.ScaledBy(pa));
             Vector ref;
-            if(pi.Equals(b0.Plus(db.ScaledBy(pb)))) {
+            DoArcForAngle(a0, da, b0, db, disp.offset, &ref);
-                ref = pi.Plus(disp.offset);
-                // We draw in a coordinate system centered at pi, with
-                // basis vectors da and dna.
-                da = da.WithMagnitude(1); dna = dna.WithMagnitude(1);
-                Vector rm = ref.Minus(pi);
-                double rda = rm.Dot(da), rdna = rm.Dot(dna);
-                double r = sqrt(rda*rda + rdna*rdna);
-                double c = (da.Dot(db))/(da.Magnitude()*db.Magnitude());
-                double thetaf = acos(c);
-
-                Vector m = da.ScaledBy(cos(thetaf/2)).Plus(
-                           dna.ScaledBy(sin(thetaf/2)));
-                if(m.Dot(rm) < 0) {
-                    da = da.ScaledBy(-1); dna = dna.ScaledBy(-1);
-                }
-
-                Vector prev = da.ScaledBy(r).Plus(pi);
-                int i, n = 30;
-                for(i = 0; i <= n; i++) {
-                    double theta = (i*thetaf)/n;
-                    Vector p = da. ScaledBy(r*cos(theta)).Plus(
-                               dna.ScaledBy(r*sin(theta))).Plus(pi);
-                    LineDrawOrGetDistance(prev, p);
-                    prev = p;
-                }
-
-                double tl = atan2(rm.Dot(gu), rm.Dot(gr));
-                double adj = EllipticalInterpolation(
-                    glxStrWidth(Label())/2, glxStrHeight()/2, tl);
-                ref = ref.Plus(rm.WithMagnitude(adj + 3/SS.GW.scale));
-            } else {
-                // The lines are skew; no wonderful way to illustrate that.
-                ref = a->VectorGetRefPoint().Plus(b->VectorGetRefPoint());
-                ref = ref.ScaledBy(0.5).Plus(disp.offset);
-                glPushMatrix();
-                    gu = gu.WithMagnitude(1);
-                    glxTranslatev(ref.Plus(gu.ScaledBy(-1.5*glxStrHeight())));
-                    glxOntoWorkplane(gr, gu);
-                    glxWriteTextRefCenter("angle between skew lines");
-                glPopMatrix();
-            }
-            
             DoLabel(ref, labelPos, gr, gu);
             break;
         }

file.cpp

@@ -129,9 +129,10 @@ const SolveSpace::SaveTable SolveSpace::SAVED[] = {
     { 'c',  "Constraint.valA",          'f',    &(SS.sv.c.valA)               },
     { 'c',  "Constraint.ptA.v",         'x',    &(SS.sv.c.ptA.v)              },
     { 'c',  "Constraint.ptB.v",         'x',    &(SS.sv.c.ptB.v)              },
-    { 'c',  "Constraint.ptC.v",         'x',    &(SS.sv.c.ptC.v)              },
     { 'c',  "Constraint.entityA.v",     'x',    &(SS.sv.c.entityA.v)          },
     { 'c',  "Constraint.entityB.v",     'x',    &(SS.sv.c.entityB.v)          },
+    { 'c',  "Constraint.entityC.v",     'x',    &(SS.sv.c.entityC.v)          },
+    { 'c',  "Constraint.entityD.v",     'x',    &(SS.sv.c.entityD.v)          },
     { 'c',  "Constraint.other",         'b',    &(SS.sv.c.other)              },
     { 'c',  "Constraint.reference",     'b',    &(SS.sv.c.reference)          },
     { 'c',  "Constraint.comment",       'N',    &(SS.sv.c.comment)            },

generate.cpp

@@ -109,9 +109,10 @@ bool SolveSpace::PruneConstraints(hGroup hg) {
         if(EntityExists(c->workplane) &&
            EntityExists(c->ptA) &&
            EntityExists(c->ptB) &&
-           EntityExists(c->ptC) &&
            EntityExists(c->entityA) &&
-           EntityExists(c->entityB))
+           EntityExists(c->entityB) &&
+           EntityExists(c->entityC) &&
+           EntityExists(c->entityD))
         {
             continue;
         }

sketch.h

@@ -450,6 +450,7 @@ public:
     static const int LENGTH_RATIO       =  51;
     static const int EQ_LEN_PT_LINE_D   =  52;
     static const int EQ_PT_LN_DISTANCES =  53;
+    static const int EQUAL_ANGLE        =  54;
     static const int SYMMETRIC          =  60;
     static const int SYMMETRIC_HORIZ    =  61;
     static const int SYMMETRIC_VERT     =  62;
@@ -481,9 +482,10 @@ public:
     double      valA;
     hEntity     ptA;
     hEntity     ptB;
-    hEntity     ptC;
     hEntity     entityA;
     hEntity     entityB;
+    hEntity     entityC;
+    hEntity     entityD;
     bool        other;
 
     bool        reference; // a ref dimension, that generates no eqs
@@ -511,6 +513,8 @@ public:
     void DrawOrGetDistance(Vector *labelPos);
     double EllipticalInterpolation(double rx, double ry, double theta);
     char *Label(void);
+    void DoArcForAngle(Vector a0, Vector da, Vector b0, Vector db,
+                        Vector offset, Vector *ref);
     void DoLabel(Vector ref, Vector *labelPos, Vector gr, Vector gu);
     void DoProjectedPoint(Vector *p);
     void DoEqualLenTicks(Vector a, Vector b, Vector gn);
@@ -527,6 +531,7 @@ public:
     // Some helpers when generating symbolic constraint equations
     void ModifyToSatisfy(void);
     void AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index);
+    static Expr *DirectionCosine(hEntity wrkpl, ExprVector ae, ExprVector be);
     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);