diff --git a/CMakeLists.txt b/CMakeLists.txt index 80bf90dc..ecef9c5b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -185,6 +185,9 @@ endif() message(STATUS "Using in-tree libdxfrw") add_subdirectory(extlib/libdxfrw) +message(STATUS "Using in-tree eigen") +include_directories(extlib/eigen) + message(STATUS "Using in-tree mimalloc") set(MI_OVERRIDE OFF CACHE BOOL "") set(MI_BUILD_SHARED OFF CACHE BOOL "") diff --git a/src/solvespace.h b/src/solvespace.h index da0b827b..5f3552e4 100644 --- a/src/solvespace.h +++ b/src/solvespace.h @@ -34,6 +34,10 @@ #include #include +#define EIGEN_NO_DEBUG +#undef Success +#include "Eigen/SparseCore" + // We declare these in advance instead of simply using FT_Library // (defined as typedef FT_LibraryRec_* FT_Library) because including // freetype.h invokes indescribable horrors and we would like to avoid @@ -233,37 +237,32 @@ public: // The system Jacobian matrix struct { // The corresponding equation for each row - hEquation eq[MAX_UNKNOWNS]; + std::vector eq; // The corresponding parameter for each column - hParam param[MAX_UNKNOWNS]; + std::vector param; // We're solving AX = B int m, n; struct { - Expr *sym[MAX_UNKNOWNS][MAX_UNKNOWNS]; - double num[MAX_UNKNOWNS][MAX_UNKNOWNS]; - } A; + Eigen::SparseMatrix *sym; + Eigen::SparseMatrix *num; + } A; - double scale[MAX_UNKNOWNS]; - - // Some helpers for the least squares solve - double AAt[MAX_UNKNOWNS][MAX_UNKNOWNS]; - double Z[MAX_UNKNOWNS]; - - double X[MAX_UNKNOWNS]; + Eigen::VectorXd scale; + Eigen::VectorXd X; struct { - Expr *sym[MAX_UNKNOWNS]; - double num[MAX_UNKNOWNS]; - } B; + std::vector sym; + Eigen::VectorXd num; + } B; } mat; - static const double RANK_MAG_TOLERANCE, CONVERGE_TOLERANCE; + static const double CONVERGE_TOLERANCE; int CalculateRank(); bool TestRank(int *dof = NULL); - static bool SolveLinearSystem(double X[], double A[][MAX_UNKNOWNS], - double B[], int N); + static bool SolveLinearSystem(const Eigen::SparseMatrix &A, + const Eigen::VectorXd &B, Eigen::VectorXd *X); bool SolveLeastSquares(); bool WriteJacobian(int tag); diff --git a/src/system.cpp b/src/system.cpp index 00f84c45..a3327532 100644 --- a/src/system.cpp +++ b/src/system.cpp @@ -8,30 +8,32 @@ //----------------------------------------------------------------------------- #include "solvespace.h" -// This tolerance is used to determine whether two (linearized) constraints -// are linearly dependent. If this is too small, then we will attempt to -// solve truly inconsistent systems and fail. But if it's too large, then -// we will give up on legitimate systems like a skinny right angle triangle by -// its hypotenuse and long side. -const double System::RANK_MAG_TOLERANCE = 1e-4; +#include // The solver will converge all unknowns to within this tolerance. This must // always be much less than LENGTH_EPS, and in practice should be much less. const double System::CONVERGE_TOLERANCE = (LENGTH_EPS/(1e2)); bool System::WriteJacobian(int tag) { + // Clear all + mat.param.clear(); + mat.eq.clear(); + mat.B.sym.clear(); - int j = 0; - for(auto &p : param) { - if(j >= MAX_UNKNOWNS) - return false; - - if(p.tag != tag) - continue; - mat.param[j] = p.h; - j++; + for(Param &p : param) { + if(p.tag != tag) continue; + mat.param.push_back(p.h); } - mat.n = j; + mat.n = mat.param.size(); + + for(Equation &e : eq) { + if(e.tag != tag) continue; + mat.eq.push_back(&e); + } + mat.m = mat.eq.size(); + delete mat.A.sym; + mat.A.sym = new Eigen::SparseMatrix(mat.m, mat.n); + mat.A.sym->reserve(Eigen::VectorXi::Constant(mat.n, 10)); // Fill the param id to index map std::map paramToIndex; @@ -39,22 +41,15 @@ bool System::WriteJacobian(int tag) { paramToIndex[mat.param[j].v] = j; } - int i = 0; - Expr *zero = Expr::From(0.0); - for(auto &e : eq) { - if(i >= MAX_UNKNOWNS) return false; - - if(e.tag != tag) - continue; - - mat.eq[i] = e.h; - Expr *f = e.e->FoldConstants(); + if(mat.eq.size() >= MAX_UNKNOWNS) { + return false; + } + for(size_t i = 0; i < mat.eq.size(); i++) { + Equation *e = mat.eq[i]; + if(e->tag != tag) continue; + Expr *f = e->e->FoldConstants(); f = f->DeepCopyWithParamsAsPointers(¶m, &(SK.param)); - for(j = 0; j < mat.n; j++) { - mat.A.sym[i][j] = zero; - } - List paramsUsed = {}; f->ParamsUsedList(¶msUsed); @@ -65,24 +60,28 @@ bool System::WriteJacobian(int tag) { pd = pd->FoldConstants(); if(pd->IsZeroConst()) continue; - mat.A.sym[i][j->second] = pd; + mat.A.sym->insert(i, j->second) = pd; } paramsUsed.Clear(); - mat.B.sym[i] = f; - i++; + mat.B.sym.push_back(f); } - mat.m = i; - return true; } void System::EvalJacobian() { - int i, j; - for(i = 0; i < mat.m; i++) { - for(j = 0; j < mat.n; j++) { - mat.A.num[i][j] = (mat.A.sym[i][j])->Eval(); + using namespace Eigen; + delete mat.A.num; + mat.A.num = new Eigen::SparseMatrix(mat.m, mat.n); + int size = mat.A.sym->outerSize(); + + for(int k = 0; k < size; k++) { + for(SparseMatrix ::InnerIterator it(*mat.A.sym, k); it; ++it) { + double value = it.value()->Eval(); + if(EXACT(value == 0.0)) continue; + mat.A.num->insert(it.row(), it.col()) = value; } } + mat.A.num->makeCompressed(); } bool System::IsDragged(hParam p) { @@ -209,45 +208,15 @@ void System::SolveBySubstitution() { } //----------------------------------------------------------------------------- -// Calculate the rank of the Jacobian matrix, by Gram-Schimdt orthogonalization -// in place. A row (~equation) is considered to be all zeros if its magnitude -// is less than the tolerance RANK_MAG_TOLERANCE. +// Calculate the rank of the Jacobian matrix //----------------------------------------------------------------------------- int System::CalculateRank() { - // Actually work with magnitudes squared, not the magnitudes - double rowMag[MAX_UNKNOWNS] = {}; - double tol = RANK_MAG_TOLERANCE*RANK_MAG_TOLERANCE; - - int i, iprev, j; - int rank = 0; - - for(i = 0; i < mat.m; i++) { - // Subtract off this row's component in the direction of any - // previous rows - for(iprev = 0; iprev < i; iprev++) { - if(rowMag[iprev] <= tol) continue; // ignore zero rows - - double dot = 0; - for(j = 0; j < mat.n; j++) { - dot += (mat.A.num[iprev][j]) * (mat.A.num[i][j]); - } - for(j = 0; j < mat.n; j++) { - mat.A.num[i][j] -= (dot/rowMag[iprev])*mat.A.num[iprev][j]; - } - } - // Our row is now normal to all previous rows; calculate the - // magnitude of what's left - double mag = 0; - for(j = 0; j < mat.n; j++) { - mag += (mat.A.num[i][j]) * (mat.A.num[i][j]); - } - if(mag > tol) { - rank++; - } - rowMag[i] = mag; - } - - return rank; + using namespace Eigen; + if(mat.n == 0 || mat.m == 0) return 0; + SparseQR , COLAMDOrdering> solver; + solver.compute(*mat.A.num); + int result = solver.rank(); + return result; } bool System::TestRank(int *dof) { @@ -259,69 +228,25 @@ bool System::TestRank(int *dof) { return jacobianRank == mat.m; } -bool System::SolveLinearSystem(double X[], double A[][MAX_UNKNOWNS], - double B[], int n) +bool System::SolveLinearSystem(const Eigen::SparseMatrix &A, + const Eigen::VectorXd &B, Eigen::VectorXd *X) { - // 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 = 0; - double max, temp; - - for(i = 0; i < n; 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 < n; ip++) { - if(fabs(A[ip][i]) > max) { - imax = ip; - max = fabs(A[ip][i]); - } - } - // Don't give up on a singular matrix unless it's really bad; the - // assumption code is responsible for identifying that condition, - // so we're not responsible for reporting that error. - if(fabs(max) < 1e-20) continue; - - // Swap row imax with row i - for(jp = 0; jp < n; jp++) { - swap(A[i][jp], A[imax][jp]); - } - swap(B[i], B[imax]); - - // For rows i+1 and greater, eliminate the term in column i. - for(ip = i+1; ip < n; ip++) { - temp = A[ip][i]/A[i][i]; - - for(jp = i; jp < n; jp++) { - A[ip][jp] -= temp*(A[i][jp]); - } - B[ip] -= temp*B[i]; - } - } - - // We've put the matrix in upper triangular form, so at this point we - // can solve by back-substitution. - for(i = n - 1; i >= 0; i--) { - if(fabs(A[i][i]) < 1e-20) continue; - - temp = B[i]; - for(j = n - 1; j > i; j--) { - temp -= X[j]*A[i][j]; - } - X[i] = temp / A[i][i]; - } - - return true; + if(A.outerSize() == 0) return true; + using namespace Eigen; + SparseQR, COLAMDOrdering> solver; + //SimplicialLDLT> solver; + solver.compute(A); + *X = solver.solve(B); + return (solver.info() == Success); } bool System::SolveLeastSquares() { - int r, c, i; - + using namespace Eigen; // Scale the columns; this scale weights the parameters for the least // squares solve, so that we can encourage the solver to make bigger // changes in some parameters, and smaller in others. - for(c = 0; c < mat.n; c++) { + mat.scale = VectorXd(mat.n); + for(int c = 0; c < mat.n; c++) { if(IsDragged(mat.param[c])) { // It's least squares, so this parameter doesn't need to be all // that big to get a large effect. @@ -329,31 +254,25 @@ bool System::SolveLeastSquares() { } else { mat.scale[c] = 1; } - for(r = 0; r < mat.m; r++) { - mat.A.num[r][c] *= mat.scale[c]; } - } - // Write A*A' - for(r = 0; r < mat.m; r++) { - for(c = 0; c < mat.m; c++) { // yes, AAt is square - double sum = 0; - for(i = 0; i < mat.n; i++) { - sum += mat.A.num[r][i]*mat.A.num[c][i]; + int size = mat.A.sym->outerSize(); + for(int k = 0; k < size; k++) { + for(SparseMatrix::InnerIterator it(*mat.A.num, k); it; ++it) { + it.valueRef() *= mat.scale[it.col()]; } - mat.AAt[r][c] = sum; } - } - if(!SolveLinearSystem(mat.Z, mat.AAt, mat.B.num, mat.m)) return false; + SparseMatrix AAt = *mat.A.num * mat.A.num->transpose(); + AAt.makeCompressed(); + VectorXd z(mat.n); - // And multiply that by A' to get our solution. - for(c = 0; c < mat.n; c++) { - double sum = 0; - for(i = 0; i < mat.m; i++) { - sum += mat.A.num[i][c]*mat.Z[i]; - } - mat.X[c] = sum * mat.scale[c]; + if(!SolveLinearSystem(AAt, mat.B.num, &z)) return false; + + mat.X = mat.A.num->transpose() * z; + + for(int c = 0; c < mat.n; c++) { + mat.X[c] *= mat.scale[c]; } return true; } @@ -365,6 +284,7 @@ bool System::NewtonSolve(int tag) { int i; // Evaluate the functions at our operating point. + mat.B.num = Eigen::VectorXd(mat.m); for(i = 0; i < mat.m; i++) { mat.B.num[i] = (mat.B.sym[i])->Eval(); } @@ -583,13 +503,12 @@ SolveResult System::Solve(Group *g, int *rank, int *dof, List *bad, didnt_converge: SK.constraint.ClearTags(); - // Not using range-for here because index is used in additional ways - for(i = 0; i < eq.n; i++) { + for(i = 0; i < mat.eq.size(); i++) { if(fabs(mat.B.num[i]) > CONVERGE_TOLERANCE || IsReasonable(mat.B.num[i])) { // This constraint is unsatisfied. - if(!mat.eq[i].isFromConstraint()) continue; + if(!mat.eq[i]->h.isFromConstraint()) continue; - hConstraint hc = mat.eq[i].constraint(); + hConstraint hc = mat.eq[i]->h.constraint(); ConstraintBase *c = SK.constraint.FindByIdNoOops(hc); if(!c) continue; // Don't double-show constraints that generated multiple @@ -637,6 +556,10 @@ void System::Clear() { param.Clear(); eq.Clear(); dragged.Clear(); + delete mat.A.num; + mat.A.num = NULL; + delete mat.A.sym; + mat.A.sym = NULL; } void System::MarkParamsFree(bool find) {