Start of robustifying Sketch::solve()

src/Sketch/src/CircularArc.h

@@ -15,7 +15,19 @@ public:
 
     CircularArc(CircularArc const *c) : Curve(c->Start, c->End, c->ForConstruction), Center(c->Center), Radius(c->Radius) {};
 
-    CircularArc(std::shared_ptr<Vertex const> v0, std::shared_ptr<Vertex const> v1, std::shared_ptr<Vertex const> c, bool fc = false) : Curve(v0, v1, fc), Center(c) {};
+    CircularArc(std::shared_ptr<Vertex const> v0, std::shared_ptr<Vertex const> v1, std::shared_ptr<Vertex const> c, bool fc = false) : Curve(v0, v1, fc), Center(c) {
+        double xc = c->x();
+        double yc = c->y();
+        double x0 = v0->x();
+        double y0 = v0->y();
+        double x1 = v1->x();
+        double y1 = v1->y();
+
+        double r0 = sqrt((xc - x0) * (xc - x0) + (yc - y0) * (yc - y0));
+        double r1 = sqrt((xc - x1) * (xc - x1) + (yc - y1) * (yc - y1));
+
+        Radius = std::make_shared<Variable>((r0 + r1) / 2.0);
+    };
 
     CircularArc(std::shared_ptr<Vertex const> v0, std::shared_ptr<Vertex const> v1, std::shared_ptr<Vertex const> c, double r, bool fc = false) : Curve(v0, v1, fc), Center(c), Radius(std::make_shared<Variable const>(r)) {};
 
@@ -34,7 +46,7 @@ public:
 
     double da(double s, bool orientation) const override;
 
-    double length() const override { throw; };
+    double length() const override { return radius() * arc_angle(); };
 
     double radius() const { return Radius->value(); };
 

src/Sketch/src/Sketch.cpp

+#include <random>
+
 #include "Sketch.h"
 #include "Vertex.h"
 #include "Curve.h"
@@ -10,47 +12,159 @@
 #include "Variable.h"
 #include "doublen.h"
 
+#include <iostream>
+
 double Sketch::solve() {
-    // #TODO: Tolerance based iteration convergence monitoring
-    // #TODO: Use sparse matrix representation for Jacobian
+    // TODO: Time to finish the bells and whistles here
+    // TODO: Use sparse matrix representation for Jacobian, (this is lower priority since typical uses will not produce very large matrices)
+    // TODO: Add residual instrumentation to tests
+
+    if (NumVariables < NumEquations) {
+        std::cerr << "Variables = " << NumVariables << ", Equations = " << NumEquations << ". Number of variables is less than the number of equations. The sketch may be over-constrained." << std::endl;
+    }
 
     Eigen::VectorXd delta = Eigen::VectorXd::Zero(NumVariables);
+    Eigen::VectorXd x = Eigen::VectorXd::Zero(NumVariables);
     Eigen::VectorXd r = Eigen::VectorXd::Zero(NumEquations);
     Eigen::VectorXd dr = Eigen::VectorXd::Zero(NumEquations);
     Eigen::MatrixXd J = Eigen::MatrixXd::Zero(NumEquations, NumVariables);
-    Eigen::MatrixXd JJT = Eigen::MatrixXd::Zero(NumEquations, NumEquations);
-    Eigen::FullPivLU<Eigen::MatrixXd> LU;
-
-    for (size_t j = 0; j < 32; ++j) {
-        // TODO: Use QR or LDLT decomposition instead
-        // TODO: Check for over/underdetermined system
-        // TODO: Try extracting sub matrix and solving
-        // TODO: Extract newton_update over Verticies, Curves, and Constraints into a private method
-
-        J.setZero();
-
+    //Eigen::MatrixXd JJT = Eigen::MatrixXd::Zero(NumEquations, NumEquations);
+    //Eigen::FullPivHouseholderQR<Eigen::MatrixXd> QR;
+    //Eigen::FullPivLU<Eigen::MatrixXd> LU;
+
+    // Calculate scale parameter for tolerances
+    double scale = 0.0;
+    if (Curves.size() > 0) {
+        for (size_t i = 0;i!= Curves.size();++i){
+            scale = std::max(scale, Curves[i]->length());
+        }
+    } else if (Verticies.size() > 0) {
+        double vmax = DBL_MIN;
+        double vmin = DBL_MAX;
         for (size_t i = 0; i != Verticies.size(); ++i) {
-            Verticies[i]->update(J, r);
+            vmax = std::max(vmax, Verticies[i]->x());
+            vmin = std::min(vmin, Verticies[i]->x());
+
+            vmax = std::max(vmax, Verticies[i]->y());
+            vmin = std::max(vmin, Verticies[i]->y());
         }
 
-        for (size_t i = 0; i != Curves.size(); ++i) {
-            Curves[i]->update(J, r);
+        if (vmax != vmin) {
+            scale = vmax - vmin;
+        } else {
+            scale = 1.0;
         }
+    }
+
+    // Initial residual calculation
+    for (size_t i = 0; i != Verticies.size(); ++i) {
+        Verticies[i]->update(J, r);
+    }
+
+    for (size_t i = 0; i != Curves.size(); ++i) {
+        Curves[i]->update(J, r);
+    }
+
+    for (size_t i = 0; i != Constraints.size(); ++i) {
+        Constraints[i]->update(J, r);
+    }
 
-        for (size_t i = 0; i != Constraints.size(); ++i) {
-            Constraints[i]->update(J, r);
+    // Setup iteration
+    double residual_norm{r.norm() / scale};
+    double residual_tol = std::sqrt(NumEquations + 1.0) * DBL_EPSILON;
+    int armijo_int{0};
+    long max_rank(std::min(NumEquations, NumVariables));
+    for (size_t j = 0; residual_norm > residual_tol && j != (8 * sizeof(double)); ++j) {
+        // TODO: Try extracting sub matrix and solving (Certain constraints are known to eliminate variables (Radius, Fixation), if the matrix can be permuted so that a submatrix on the block diagonal is lower-triangular, then a subsystem of equations can be solved independently of the remaining equations. Moreover, if the jacobian can be permuted into a block lower triangular form, subsets of equations can be solved blockwise)
+        // TODO: Extract newton_update over Verticies, Curves, and Constraints into a private method
+        // TODO: Extract armijo_update into a private method
+        // TODO: Factor out rank deficiency perturbation to be independent of QR/LU/LDL^T choice
+
+        Eigen::FullPivHouseholderQR<Eigen::MatrixXd> QR = J.fullPivHouseholderQr();
+        delta = QR.solve(r) * std::pow(2.0, (double) armijo_int);
+        long iter_rank = QR.rank();
+
+        if (iter_rank < max_rank) {
+            max_rank = iter_rank;
+            std::cerr << "Rank = " << iter_rank << ", Equations = " << NumEquations << ", Variables = " << NumVariables << ", Iteration = " << j
+                      << ". The jacobian rank is less than the number of equations. Some sketch elements may be over-constrained." << std::endl;
+
+            // Randomly perturb the delta vector to prevent stagnation at inflection point
+            std::random_device rdev;
+            std::mt19937 rgen(rdev());
+            auto rdist = std::uniform_real_distribution<>(-FLT_EPSILON * scale, FLT_EPSILON * scale);
+            for (size_t i = 0; i != Variables.size(); ++i) {
+                delta(Variables[i]->get_index()) += rdist(rgen);
+            }
         }
 
-        JJT = J * J.transpose();
-        LU = JJT.fullPivLu();
-        delta = J.transpose() * LU.solve(r);
+        // Update Jacobian, residual, and Armijo step-size
+        double iter_norm{DBL_MAX};
+        int armijo_dir{0};
+        while (iter_norm > residual_norm && residual_norm > std::sqrt(NumEquations + 1) * DBL_EPSILON) {
+            for (size_t i = 0; i < Variables.size(); ++i) {
+                Variables[i]->update(delta);
+            }
+
+            J.setZero();
+
+            for (size_t i = 0; i != Verticies.size(); ++i) {
+                Verticies[i]->update(J, r);
+            }
+
+            for (size_t i = 0; i != Curves.size(); ++i) {
+                Curves[i]->update(J, r);
+            }
+
+            for (size_t i = 0; i != Constraints.size(); ++i) {
+                Constraints[i]->update(J, r);
+            }
+
+            iter_norm = r.norm() / scale;
+
+            if (armijo_dir == 0) {
+                if (iter_norm < residual_norm) {
+                    armijo_dir = +1;
+                } else {
+                    delta = -delta;
+                    armijo_dir = -1;
+                }
+            }
+
+            if (armijo_dir == 1) {
+                if (iter_norm < residual_norm) {
+                    if (armijo_int == 0) {
+                        residual_norm = iter_norm;
+                        break;
+                    } else {
+                        delta /= 2.0;
+                        armijo_int += 1;
+                    }
+                } else {
+                    delta *= -2.0;
+                    armijo_dir = -1;
+                }
+            }
 
-        for (size_t i = 0; i < Variables.size(); ++i) {
-            Variables[i]->update(delta);
+            if (armijo_dir == -1) {
+                if (iter_norm > residual_norm) {
+                    if (armijo_int == -(8 * sizeof(double))) {
+                        std::cerr << "Residual norm was not reduced for Armijo step-sizes greater than 2^-(8*sizeof(double))" << std::endl;
+                        residual_norm = iter_norm;
+                        break;
+                    } else {
+                        delta /= 2.0;
+                        armijo_int -= 1;
+                    }
+                } else {
+                    residual_norm = iter_norm;
+                    break;
+                }
+            }
         }
     }
 
-    return r.norm();
+    return residual_norm;
 }
 
 bool Sketch::build() {
@@ -88,7 +202,7 @@ void Sketch::save_as<SaveMethod::Rasterize>(std::string path, std::string file_n
         This is a stub for visualization
     */
 
-    if(!boost::filesystem::exists(path)) {
+    if (!boost::filesystem::exists(path)) {
         boost::filesystem::create_directories(path);
     }
 

src/Sketch/src/Sketch.h

@@ -38,9 +38,9 @@ public:
 
     size_t size_verticies() const { return Verticies.size(); };
 
-    bool build();
+    bool build(); // TODO: Detailed return enum
 
-    double solve();
+    double solve(); // TODO: Detailed return enum
 
     void add_element(std::shared_ptr<Constraint> c);
 

src/Sketch/src/SketchElement.h

@@ -8,6 +8,7 @@
 #include <utility>
 #include <vector>
 #include <list>
+#include <random>
 
 #include "Eigen"
 

test/Sketch/test_CircularArc.cpp

 #include "test_Sketch.hpp"
 
 TEST(CircularArc, constructor) {
-    { //ARGS::()
-        EXPECT_NO_THROW(CircularArc c);
-    }
-
-    { //ARGS::(Vertex,Vertex,Vertex)
-        std::shared_ptr<Vertex> v0, v1, center;
-        EXPECT_NO_THROW(CircularArc c(v0, v1, center));
-    }
+    EXPECT_NO_THROW(CircularArc c);
 }
 
 TEST(CircularArc, point) {

test/Sketch/test_Constraint.cpp

@@ -234,7 +234,7 @@ TEST(Constraint, Angle) {
 
         ang->dim(a);
 
-        s.solve();
+        double res_norm = s.solve();
 
         s.save_as<SaveMethod::Rasterize>(SAVE_DIR, std::string("Constraint__Angle_LineSegment_LineSegment_")+std::to_string(i));
 
@@ -243,7 +243,7 @@ TEST(Constraint, Angle) {
         EXPECT_NEAR(v0->y(), v00->y(), TOL);
         EXPECT_NEAR(1.0, line1->length(), TOL);
 
-        EXPECT_NEAR(tan(M_PI * i / 8.0), ((v01->y() - v0->y()) / (v01->x() - v0->x())), TOL);
+        EXPECT_NEAR(sin(M_PI * i / 8.0) * (v01->x() - v0->x()), cos(M_PI * i / 8.0) * (v01->y() - v0->y()), TOL); // Tests line sloep. Avoid divide by zero error when calling tan(+/-M_PI / 2.0) by using tan = sin/cos
         EXPECT_NEAR(cos(M_PI * i / 8.0), v01->x(), TOL);
         EXPECT_NEAR(sin(M_PI * i / 8.0), v01->y(), TOL);
     }
@@ -360,7 +360,8 @@ TEST(Constraint, Distance_LineSegment) {
 
         auto l0 = s.new_element<LineSegment>(v00, v01);
         auto l1 = s.new_element<LineSegment>(v10, v11);
-        auto d = s.new_element<Distance<LineSegment>>(l0, l1, 1.0);
+
+        auto d01 = s.new_element<Distance<LineSegment>>(l0, l1, 1.0);
 
         s.solve();
 

test/Sketch/test_MirrorCopy.cpp

@@ -78,7 +78,7 @@ TEST(MirrorCopy, nonoverlapping) {
 
     s.save_as<SaveMethod::Rasterize>(SAVE_DIR, "Pattern__Mirror_nonoverlapping_square");
 
-    s.solve();
+    double residual = s.solve();
     s.build();
 
     // Run Tests
@@ -91,7 +91,7 @@ TEST(MirrorCopy, nonoverlapping) {
     // Change elements
     {
         s.new_element<Length>(l0, 0.5);
-        s.solve();
+        residual = s.solve();
         s.build();
 
         s.save_as<SaveMethod::Rasterize>(SAVE_DIR, "Pattern__Mirror_nonoverlapping_trapezoid");
@@ -123,13 +123,12 @@ TEST(MirrorCopy, overlapping) {
         s.new_element<Fixation>(v0);
         s.new_element<Fixation>(v3);
 
-        // std::vector<const Curve *> vec{&l0, &l1, &l2, &l3};
         auto mvec = s.curves();
         auto mc0 = s.new_element<MirrorCopy>(mvec, l3, remove_internal);
 
         s.save_as<SaveMethod::Rasterize>(SAVE_DIR, std::string("Mirror__overlapping_parallelogram")+std::to_string(remove_internal));
 
-        s.solve();
+        double residual = s.solve();
         s.build();
 
         // Run Tests
@@ -142,7 +141,7 @@ TEST(MirrorCopy, overlapping) {
         // Change elements
         {
             s.new_element<Length>(l1, 0.5);
-            s.solve();
+            residual = s.solve();
             s.build();
 
             s.save_as<SaveMethod::Rasterize>(SAVE_DIR, std::string("Mirror__overlapping_trapezoid")+std::to_string(remove_internal));
@@ -179,11 +178,10 @@ TEST(MirrorCopy, multiple_overlapping) {
         //l3.ForConstruction = true;
         s.new_element<Coincident<LineSegment>>(v5, l3);
 
-        //std::vector<const Curve *> vec{&l0, &l1, &l2, &l3, &l4, &l5, &l6};
         auto mvec = s.curves();
         s.new_element<MirrorCopy>(mvec, l3, remove_internal);
 
-        s.solve();
+        double residual = s.solve();
         s.build();
 
         s.save_as<SaveMethod::Rasterize>(SAVE_DIR, std::string("Mirror__multiple_overlapping_0_")+std::to_string(remove_internal));
@@ -198,7 +196,7 @@ TEST(MirrorCopy, multiple_overlapping) {
         // Change elements
         {
             s.new_element<Length>(l6, 2.0);
-            s.solve();
+            residual = s.solve();
             s.build();
 
             s.save_as<SaveMethod::Rasterize>(SAVE_DIR, std::string("Mirror__multiple_overlapping_1_")+std::to_string(remove_internal));

test/Sketch/test_Sketch.hpp

@@ -7,7 +7,7 @@
 #include "Sketch.hpp"
 #include "gtest.h"
 
-#define TOL FLT_EPSILON //#TODO: Tolerance is limited by accuracy of tangency constraint
+#define TOL FLT_EPSILON // TODO: Tolerance is limited by accuracy of tangency constraint
 #define SAVE_DIR "./test/output/Sketch/"
 
 void test_sketch_size(Sketch &s, size_t nverts, size_t ncurves, size_t nconstraints, size_t ncontours);

test/UseCases/test_Stator.cpp

 #include "test_UseCases.hpp"
 
-TEST(Stator, Suite0) {
+TEST(Stator, 0) {
     Sketch sketch;
 
     size_t Np{8};
@@ -71,7 +71,7 @@ TEST(Stator, Suite0) {
 
     Mesh mesh{sketch};
 
-    mesh.MinimumElementQuality = 0 * M_SQRT1_2;
+    mesh.MinimumElementQuality = M_SQRT1_2;
     mesh.MaximumElementSize = 2.5;
     mesh.MinimumElementSize = 0.25;
 
@@ -84,4 +84,154 @@ TEST(Stator, Suite0) {
 
     mesh.refine();
     mesh.save_as(MDIR, "stator0_refined");
+}
+
+TEST(Stator, 1) {
+    Sketch sketch;
+
+    // Parameters
+    size_t Np{8}; // number of poles
+    size_t Nt{6 * Np}; // number of teeth
+
+    double rsi{80e-3}; // stator inner radius
+    double rso{130e-3}; // stator outer radius
+    double at2_deg{180.0 / Nt}; // half tooth pitch angle in degrees
+    double at2_rad{M_PI / Nt}; // half tooth pitch angle in radians
+
+    double sow{1.0e-3}; // slot opening width
+    double sod{1.5e-3}; // slot opening depth
+
+    double ptw{0.66}; // tooth width percentage of rsi * cos(at2_rad)
+    double dtw{ptw * rsi * sin(at2_rad)};
+
+    double pbi{0.3}; // backiron width percentage of (rso - rsi)
+    double dbi{pbi * (rso - rsi)};
+
+    // Verticies
+    auto v0 = sketch.new_element<Vertex>(0.0, 0.0);
+    auto f0 = sketch.new_element<Fixation>(v0);
+
+    auto v1 = sketch.new_element<Vertex>(rsi, 0.0);
+    auto v2 = sketch.new_element<Vertex>(rso, 0.0);
+    auto v3 = sketch.new_element<Vertex>(rso * cos(at2_rad), rso * sin(at2_rad));
+    auto v4 = sketch.new_element<Vertex>(rsi * cos(at2_rad), rsi * sin(at2_rad));
+
+    auto v5 = sketch.new_element<Vertex>(rsi * cos(at2_rad), rsi * sin(at2_rad) - sod); // approximate dimensions
+    auto v6 = sketch.new_element<Vertex>(rsi * cos(at2_rad) + sod, rsi * sin(at2_rad) - sod); // approximate dimensions
+    auto v7 = sketch.new_element<Vertex>((rsi + sod) * cos(at2_rad), (rsi + sod) * sin(at2_rad));
+
+    auto v8 = sketch.new_element<Vertex>(rsi + 2 * sod, dtw);
+    auto v9 = sketch.new_element<Vertex>(rso - dbi, dtw);
+    auto v10 = sketch.new_element<Vertex>((rso - dbi) * cos(at2_rad), (rso - dbi) * sin(at2_rad));
+
+    auto v11 = sketch.new_element<Vertex>(rsi + 2 * sod, rsi * sin(at2_rad) - sod);
+    auto v12 = sketch.new_element<Vertex>((rso - 2 * dbi) * cos(at2_rad), (rso - 2 * dbi) * sin(at2_rad));
+
+    // LineSegment Curves
+    auto l_0_4 = sketch.new_element<LineSegment>(v0, v4, true);
+
+    auto l_1_2 = sketch.new_element<LineSegment>(v1, v2);
+    auto l_4_7 = sketch.new_element<LineSegment>(v4, v7);
+    auto l_7_10 = sketch.new_element<LineSegment>(v7, v10);
+    auto l_10_3 = sketch.new_element<LineSegment>(v10, v3);
+
+    auto l_5_6 = sketch.new_element<LineSegment>(v5, v6);
+    auto l_6_7 = sketch.new_element<LineSegment>(v6, v7);
+
+    auto l_8_9 = sketch.new_element<LineSegment>(v8, v9);
+
+    // Circle Curves
+    auto c_1_5_0 = sketch.new_element<CircularArc>(v1, v5, v0, rsi);
+    auto c_5_4_0 = sketch.new_element<CircularArc>(v5, v4, v0, rsi);
+    auto c_2_3_0 = sketch.new_element<CircularArc>(v2, v3, v0, rso);
+
+    auto c_6_8_11 = sketch.new_element<CircularArc>(v6, v8, v11);
+
+    auto c_9_10_12 = sketch.new_element<CircularArc>(v9, v10, v12);
+
+    // Angle Constraints
+    auto angle_1_2_0_4 = sketch.new_element<Angle>(l_1_2, l_0_4, at2_deg);
+    auto angle_1_2_4_7 = sketch.new_element<Angle>(l_1_2, l_4_7, at2_deg);
+    auto angle_1_2_7_10 = sketch.new_element<Angle>(l_1_2, l_7_10, at2_deg);
+    auto angle_1_2_10_3 = sketch.new_element<Angle>(l_1_2, l_10_3, at2_deg);
+
+    // Distance Constraint
+    auto distance_5_6_4_7 = sketch.new_element<Distance<LineSegment>>(l_5_6, l_4_7, sow);
+    auto distance_1_2_8_9 = sketch.new_element<Distance<LineSegment>>(l_1_2, l_8_9, dtw);
+
+    // Coincident Constraints
+    auto coincident_0_1_2 = sketch.new_element<Coincident<LineSegment>>(v0, l_1_2);
+
+    auto coincident_12_7_10 = sketch.new_element<Coincident<LineSegment>>(v12, l_7_10);
+
+    // Horizontal Constraints
+    auto horziontal_1_2 = sketch.new_element<Horizontal>(l_1_2);
+
+    // Length Constraints
+    auto length_5_6 = sketch.new_element<Length>(l_5_6, sod);
+    auto length_4_7 = sketch.new_element<Length>(l_4_7, sod);
+    auto length_10_3 = sketch.new_element<Length>(l_10_3, dbi);
+
+    // Radius Constraints
+    auto radius_1_5_0 = sketch.new_element<Radius>(c_1_5_0, rsi);
+    auto radius_2_3_0 = sketch.new_element<Radius>(c_2_3_0, rso);
+
+    // Tangency Constraints
+    auto tangent_9_10_12_8_9 = sketch.new_element<Tangency>(c_9_10_12, l_8_9);
+
+    auto tangent_6_8_11_8_9 = sketch.new_element<Tangency>(c_6_8_11, l_8_9);
+
+    auto tangent_6_8_11_6_7 = sketch.new_element<Tangency>(c_6_8_11, l_6_7);
+
+    // Solve
+    double residual_norm = sketch.solve();
+
+    sketch.save_as<SaveMethod::Rasterize>(SDIR, "stator1_0_half");
+
+    // MirrorCopy
+    auto mccs = sketch.curves();
+
+    auto mirror_copy = sketch.new_element<MirrorCopy>(mccs, l_0_4, true);
+
+    // Solve Again
+    residual_norm = sketch.solve();
+
+    sketch.save_as<SaveMethod::Rasterize>(SDIR, "stator1_1_mirror");
+
+    // RotateCopy
+    auto rccs = sketch.curves();
+
+    auto rotate_copy = sketch.new_element<RotateCopy>(rccs, v0, at2_deg * 2.0, 1, true); // Can do 5 copies for full pole, but is slow at -O0
+
+    // Solve Again
+    residual_norm = sketch.solve();
+
+    sketch.save_as<SaveMethod::Rasterize>(SDIR, "stator1_2_rotate");
+
+    // Solve Again
+    radius_1_5_0->dim(radius_1_5_0->dim() + 10e-3);
+
+    residual_norm = sketch.solve();
+
+    sketch.save_as<SaveMethod::Rasterize>(SDIR, "stator1_2_rotate_radius");
+
+    // Build
+    bool build_result = sketch.build();
+
+    // Create Mesh
+    Mesh mesh{sketch};
+
+    mesh.MinimumElementQuality = M_SQRT1_2;
+    mesh.MaximumElementSize = 2.0e-3;
+    mesh.MinimumElementSize = 0.20e-3;
+
+    mesh.create();
+
+    EXPECT_TRUE(edges_are_valid(mesh));
+    EXPECT_TRUE(edges_are_optimal(mesh));
+
+    mesh.save_as(MDIR, "stator1");
+
+    mesh.refine();
+    mesh.save_as(MDIR, "stator1_refined");
 }
\ No newline at end of file