Rewrite equations generated for parallel constraints (in 3d).

Before this commit, parallel constraints in 3d are fragile:
constraints that are geometrically fine can end up singular anyway
because VectorsParallel() pivots wrong but converges anyway.
After this commit, much like in cc07058, the constraints are written
in a different form: instead of trying to remove two degrees of
freedom out of three, all three are removed, and one added; namely,
the constraint introduces a free parameter, signed length ratio.

CHANGELOG.md

@@ -41,6 +41,7 @@ Other new features:
 Bugs fixed:
   * A point in 3d constrained to any line whose length is free no longer
     causes the line length to collapse.
+  * Lines in 3d constrained parallel are solved in a more robust way.
 
 2.3
 ---

src/constrainteq.cpp

@@ -57,6 +57,10 @@ Expr *ConstraintBase::VectorsParallel(int eq, ExprVector a, ExprVector b) {
     ssassert(false, "Unexpected index of equation");
 }
 
+ExprVector ConstraintBase::VectorsParallel3d(ExprVector a, ExprVector b, hParam p) {
+    return a.Minus(b.ScaledBy(Expr::From(p)));
+}
+
 Expr *ConstraintBase::PointLineDistance(hEntity wrkpl, hEntity hpt, hEntity hln)
 {
     EntityBase *ln = SK.GetEntity(hln);
@@ -201,8 +205,21 @@ void ConstraintBase::AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index)
     l->Add(&eq);
 }
 
+void ConstraintBase::AddEq(IdList<Equation,hEquation> *l, const ExprVector &v,
+                           int baseIndex) const {
+    AddEq(l, v.x, baseIndex);
+    AddEq(l, v.y, baseIndex + 1);
+    if(workplane.v == EntityBase::FREE_IN_3D.v) {
+        AddEq(l, v.z, baseIndex + 2);
+    }
+}
+
 void ConstraintBase::Generate(IdList<Param,hParam> *l) const {
     switch(type) {
+        case Type::PARALLEL:
+            // Only introduce a new parameter when operating in 3d
+            if(workplane.v != EntityBase::FREE_IN_3D.v) break;
+            // fallthrough
         case Type::PT_ON_LINE: {
             Param p = {};
             p.h = h.param(0);
@@ -406,9 +423,7 @@ void ConstraintBase::GenerateEquations(IdList<Equation,hEquation> *l,
             ExprVector ptOnLine = ea.Plus(eb.Minus(ea).ScaledBy(Expr::From(h.param(0))));
             ExprVector eq = ptOnLine.Minus(ep);
 
-            AddEq(l, eq.x, 0);
+            AddEq(l, eq);
-            AddEq(l, eq.y, 1);
-            if(workplane.v == EntityBase::FREE_IN_3D.v) AddEq(l, eq.z, 2);
             return;
         }
 
@@ -755,20 +770,24 @@ void ConstraintBase::GenerateEquations(IdList<Equation,hEquation> *l,
 
         case Type::PARALLEL: {
             EntityBase *ea = SK.GetEntity(entityA), *eb = SK.GetEntity(entityB);
-            if(eb->group.v != group.v) {
+            ExprVector a = ea->VectorGetExprsInWorkplane(workplane);
-                swap(ea, eb);
+            ExprVector b = eb->VectorGetExprsInWorkplane(workplane);
-            }
-            ExprVector a = ea->VectorGetExprs();
-            ExprVector b = eb->VectorGetExprs();
 
             if(workplane.v == EntityBase::FREE_IN_3D.v) {
-                AddEq(l, VectorsParallel(0, a, b), 0);
+                ExprVector eq = VectorsParallel3d(a, b, h.param(0));
-                AddEq(l, VectorsParallel(1, a, b), 1);
+                AddEq(l, eq);
             } else {
-                EntityBase *w = SK.GetEntity(workplane);
+                // We use expressions written in workplane csys, so we can assume the workplane
-                ExprVector wn = w->Normal()->NormalExprsN();
+                // normal is (0, 0, 1). We can write the equation as:
-                AddEq(l, (a.Cross(b)).Dot(wn), 0);
+                //   Expr *eq = a.Cross(b).Dot(ExprVector::From(0.0, 0.0, 1.0));
+                // but this will just result in elimination of x and y terms after dot product.
+                // We can only use the z expression:
+                //   Expr *eq = a.Cross(b).z;
+                // but it's more efficient to write it in the terms of pseudo-scalar product:
+                Expr *eq = (a.x->Times(b.y))->Minus(a.y->Times(b.x));
+                AddEq(l, eq, 0);
             }
+
             return;
         }
 

src/entity.cpp

@@ -25,23 +25,42 @@ bool EntityBase::HasVector() const {
     }
 }
 
-ExprVector EntityBase::VectorGetExprs() const {
+ExprVector EntityBase::VectorGetExprsInWorkplane(hEntity wrkpl) const {
     switch(type) {
         case Type::LINE_SEGMENT:
-            return (SK.GetEntity(point[0])->PointGetExprs()).Minus(
+            return (SK.GetEntity(point[0])->PointGetExprsInWorkplane(wrkpl)).Minus(
-                    SK.GetEntity(point[1])->PointGetExprs());
+                    SK.GetEntity(point[1])->PointGetExprsInWorkplane(wrkpl));
 
         case Type::NORMAL_IN_3D:
         case Type::NORMAL_IN_2D:
         case Type::NORMAL_N_COPY:
         case Type::NORMAL_N_ROT:
-        case Type::NORMAL_N_ROT_AA:
+        case Type::NORMAL_N_ROT_AA: {
-            return NormalExprsN();
+            ExprVector ev = NormalExprsN();
+            if(wrkpl.v == EntityBase::FREE_IN_3D.v) {
+                return ev;
+            }
+            // Get the offset and basis vectors for this weird exotic csys.
+            EntityBase *w = SK.GetEntity(wrkpl);
+            ExprVector wu = w->Normal()->NormalExprsU();
+            ExprVector wv = w->Normal()->NormalExprsV();
 
+            // Get our coordinates in three-space, and project them into that
+            // coordinate system.
+            ExprVector result;
+            result.x = ev.Dot(wu);
+            result.y = ev.Dot(wv);
+            result.z = Expr::From(0.0);
+            return result;
+        }
         default: ssassert(false, "Unexpected entity type");
     }
 }
 
+ExprVector EntityBase::VectorGetExprs() const {
+    return VectorGetExprsInWorkplane(EntityBase::FREE_IN_3D);
+}
+
 Vector EntityBase::VectorGetNum() const {
     switch(type) {
         case Type::LINE_SEGMENT:

src/file.cpp

@@ -594,6 +594,29 @@ void SolveSpaceUI::UpgradeLegacyData() {
                 }
                 break;
             }
+
+            case Constraint::Type::PARALLEL: {
+                IdList<Param,hParam> param = {};
+                c.Generate(&param);
+                bool allParamsExist = true;
+                for(Param &p : param) {
+                    if(oldParam.FindByIdNoOops(p.h) != NULL) continue;
+                    allParamsExist = false;
+                }
+                param.Clear();
+
+                if(!allParamsExist) {
+                    EntityBase *ea = SK.GetEntity(c.entityA),
+                               *eb = SK.GetEntity(c.entityB);
+                    ExprVector a = ea->VectorGetExprsInWorkplane(c.workplane);
+                    ExprVector b = eb->VectorGetExprsInWorkplane(c.workplane);
+
+                    Param *param = SK.GetParam(c.h.param(0));
+                    param->val = a.Dot(b)->Eval() / b.Dot(b)->Eval();
+                }
+                break;
+            }
+
             default:
                 break;
         }

src/sketch.h

@@ -409,6 +409,7 @@ public:
 
     bool HasVector() const;
     ExprVector VectorGetExprs() const;
+    ExprVector VectorGetExprsInWorkplane(hEntity wrkpl) const;
     Vector VectorGetNum() const;
     Vector VectorGetRefPoint() const;
     Vector VectorGetStartPoint() const;
@@ -646,11 +647,13 @@ public:
     // Some helpers when generating symbolic constraint equations
     void ModifyToSatisfy();
     void AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index) const;
+    void AddEq(IdList<Equation,hEquation> *l, const ExprVector &v, int baseIndex = 0) const;
     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);
     static Expr *VectorsParallel(int eq, ExprVector a, ExprVector b);
+    static ExprVector VectorsParallel3d(ExprVector a, ExprVector b, hParam p);
     static ExprVector PointInThreeSpace(hEntity workplane, Expr *u, Expr *v);
 
     void Clear() {}

test/constraint/parallel/free_in_3d.slvs

@@ -136,6 +136,10 @@ AddParam
 Param.h.v.=00040015
 AddParam
 
+Param.h.v.=40000001
+Param.val=5.00000000000000000000
+AddParam
+
 Request.h.v=00000001
 Request.type=100
 Request.group.v=00000001