zcy
/
solvespace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
solvespace/src/system.cpp

594 lines
18 KiB
C++
Raw Blame History

//-----------------------------------------------------------------------------
// Once we've written our constraint equations in the symbolic algebra system,
// these routines linearize them, and solve by a modified Newton's method.
// This also contains the routines to detect non-convergence or inconsistency,
// and report diagnostics to the user.
//
// Copyright 2008-2013 Jonathan Westhues.
//-----------------------------------------------------------------------------
#include "solvespace.h"
#include <Eigen/Core>
#include <Eigen/SparseQR>
// 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));
constexpr size_t LikelyPartialCountPerEq = 10;
bool System::WriteJacobian(int tag) {
// Clear all
mat.param.clear();
mat.eq.clear();
mat.A.sym.setZero();
mat.B.sym.clear();
for(Param &p : param) {
if(p.tag != tag) continue;
mat.param.push_back(p.h);
}
mat.n = mat.param.size();
for(Equation &e : eq) {
if(e.tag != tag) continue;
mat.eq.push_back(&e);
}
mat.m = mat.eq.size();
mat.A.sym.resize(mat.m, mat.n);
mat.A.sym.reserve(Eigen::VectorXi::Constant(mat.n, LikelyPartialCountPerEq));
// Fill the param id to index map
std::map<uint32_t, int> paramToIndex;
for(int j = 0; j < mat.n; j++) {
paramToIndex[mat.param[j].v] = j;
}
if(mat.eq.size() >= MAX_UNKNOWNS) {
return false;
}
std::vector<hParam> paramsUsed;
// In some experimenting, this is almost always the right size.
// Value is usually between 0 and 20, comes from number of constraints?
mat.B.sym.reserve(mat.eq.size());
for(size_t i = 0; i < mat.eq.size(); i++) {
Equation *e = mat.eq[i];
if(e->tag != tag) continue;
// Simplify (fold) then deep-copy the current equation.
Expr *f = e->e->FoldConstants();
f = f->DeepCopyWithParamsAsPointers(&param, &(SK.param));
paramsUsed.clear();
f->ParamsUsedList(&paramsUsed);
for(hParam &p : paramsUsed) {
// Find the index of this parameter
auto it = paramToIndex.find(p.v);
if(it == paramToIndex.end()) continue;
// this is the parameter index
const int j = it->second;
// compute partial derivative of f
Expr *pd = f->PartialWrt(p);
pd = pd->FoldConstants();
if(pd->IsZeroConst())
continue;
mat.A.sym.insert(i, j) = pd;
}
paramsUsed.clear();
mat.B.sym.push_back(f);
}
return true;
}
void System::EvalJacobian() {
using namespace Eigen;
mat.A.num.setZero();
mat.A.num.resize(mat.m, mat.n);
const int size = mat.A.sym.outerSize();
for(int k = 0; k < size; k++) {
for(SparseMatrix <Expr *>::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) {
const auto b = dragged.begin();
const auto e = dragged.end();
return e != std::find(b, e, p);
}
Param *System::GetLastParamSubstitution(Param *p) {
Param *current = p;
while(current->substd != NULL) {
current = current->substd;
if(current == p) {
// Break the loop
current->substd = NULL;
break;
}
}
return current;
}
void System::SortSubstitutionByDragged(Param *p) {
std::vector<Param *> subsParams;
Param *by = NULL;
Param *current = p;
while(current != NULL) {
subsParams.push_back(current);
if(IsDragged(current->h)) {
by = current;
}
current = current->substd;
}
if(by == NULL) by = p;
for(Param *p : subsParams) {
if(p == by) continue;
p->substd = by;
p->tag = VAR_SUBSTITUTED;
}
by->substd = NULL;
by->tag = 0;
}
void System::SubstituteParamsByLast(Expr *e) {
ssassert(e->op != Expr::Op::PARAM_PTR, "Expected an expression that refer to params via handles");
if(e->op == Expr::Op::PARAM) {
Param *p = param.FindByIdNoOops(e->parh);
if(p != NULL) {
Param *s = GetLastParamSubstitution(p);
if(s != NULL) {
e->parh = s->h;
}
}
} else {
int c = e->Children();
if(c >= 1) {
SubstituteParamsByLast(e->a);
if(c >= 2) SubstituteParamsByLast(e->b);
}
}
}
void System::SolveBySubstitution() {
for(auto &teq : eq) {
Expr *tex = teq.e;
if(tex->op == Expr::Op::MINUS &&
tex->a->op == Expr::Op::PARAM &&
tex->b->op == Expr::Op::PARAM)
{
hParam a = tex->a->parh;
hParam b = tex->b->parh;
if(!(param.FindByIdNoOops(a) && param.FindByIdNoOops(b))) {
// Don't substitute unless they're both solver params;
// otherwise it's an equation that can be solved immediately,
// or an error to flag later.
continue;
}
if(a.v == b.v) {
teq.tag = EQ_SUBSTITUTED;
continue;
}
Param *pa = param.FindById(a);
Param *pb = param.FindById(b);
// Take the last substitution of parameter a
// This resulted in creation of substitution chains
Param *last = GetLastParamSubstitution(pa);
last->substd = pb;
last->tag = VAR_SUBSTITUTED;
if(pb->substd != NULL) {
// Break the loops
GetLastParamSubstitution(pb);
// if b loop was broken
if(pb->substd == NULL) {
// Clear substitution
pb->tag = 0;
}
}
teq.tag = EQ_SUBSTITUTED;
}
}
//
for(Param &p : param) {
SortSubstitutionByDragged(&p);
}
// Substitute all the equations
for(auto &req : eq) {
SubstituteParamsByLast(req.e);
}
// Substitute all the parameters with last substitutions
for(auto &p : param) {
if(p.substd == NULL) continue;
p.substd = GetLastParamSubstitution(p.substd);
}
}
//-----------------------------------------------------------------------------
// Calculate the rank of the Jacobian matrix
//-----------------------------------------------------------------------------
int System::CalculateRank() {
using namespace Eigen;
if(mat.n == 0 || mat.m == 0) return 0;
SparseQR <SparseMatrix<double>, COLAMDOrdering<int>> solver;
solver.compute(mat.A.num);
int result = solver.rank();
return result;
}
bool System::TestRank(int *dof) {
EvalJacobian();
int jacobianRank = CalculateRank();
// We are calculating dof based on real rank, not mat.m.
// Using this approach we can calculate real dof even when redundant is allowed.
if(dof != NULL) *dof = mat.n - jacobianRank;
return jacobianRank == mat.m;
}
bool System::SolveLinearSystem(const Eigen::SparseMatrix <double> &A,
const Eigen::VectorXd &B, Eigen::VectorXd *X)
{
if(A.outerSize() == 0) return true;
using namespace Eigen;
SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> solver;
//SimplicialLDLT<SparseMatrix<double>> solver;
solver.compute(A);
*X = solver.solve(B);
return (solver.info() == Success);
}
bool System::SolveLeastSquares() {
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.
mat.scale = VectorXd::Ones(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.
mat.scale[c] = 1 / 20.0;
}
}
const int size = mat.A.num.outerSize();
for(int k = 0; k < size; k++) {
for(SparseMatrix<double>::InnerIterator it(mat.A.num, k); it; ++it) {
it.valueRef() *= mat.scale[it.col()];
}
}
SparseMatrix<double> AAt = mat.A.num * mat.A.num.transpose();
AAt.makeCompressed();
VectorXd z(mat.n);
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;
}
bool System::NewtonSolve(int tag) {
int iter = 0;
bool converged = false;
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();
}
do {
// And evaluate the Jacobian at our initial operating point.
EvalJacobian();
if(!SolveLeastSquares()) break;
// Take the Newton step;
// J(x_n) (x_{n+1} - x_n) = 0 - F(x_n)
for(i = 0; i < mat.n; i++) {
Param *p = param.FindById(mat.param[i]);
p->val -= mat.X[i];
if(IsReasonable(p->val)) {
// Very bad, and clearly not convergent
return false;
}
}
// Re-evalute the functions, since the params have just changed.
for(i = 0; i < mat.m; i++) {
mat.B.num[i] = (mat.B.sym[i])->Eval();
}
// Check for convergence
converged = true;
for(i = 0; i < mat.m; i++) {
if(IsReasonable(mat.B.num[i])) {
return false;
}
if(fabs(mat.B.num[i]) > CONVERGE_TOLERANCE) {
converged = false;
break;
}
}
} while(iter++ < 50 && !converged);
return converged;
}
void System::WriteEquationsExceptFor(hConstraint hc, Group *g) {
// Generate all the equations from constraints in this group
for(auto &con : SK.constraint) {
ConstraintBase *c = &con;
if(c->group != g->h) continue;
if(c->h == hc) continue;
if(c->HasLabel() && c->type != Constraint::Type::COMMENT &&
g->allDimsReference)
{
// When all dimensions are reference, we adjust them to display
// the correct value, and then don't generate any equations.
c->ModifyToSatisfy();
continue;
}
if(g->relaxConstraints && c->type != Constraint::Type::POINTS_COINCIDENT) {
// When the constraints are relaxed, we keep only the point-
// coincident constraints, and the constraints generated by
// the entities and groups.
continue;
}
c->GenerateEquations(&eq);
}
// And the equations from entities
for(auto &ent : SK.entity) {
EntityBase *e = &ent;
if(e->group != g->h) continue;
e->GenerateEquations(&eq);
}
// And from the groups themselves
g->GenerateEquations(&eq);
}
void System::FindWhichToRemoveToFixJacobian(Group *g, List<hConstraint> *bad, bool forceDofCheck) {
auto time = GetMilliseconds();
g->solved.timeout = false;
int a;
for(a = 0; a < 2; a++) {
for(auto &con : SK.constraint) {
if((GetMilliseconds() - time) > g->solved.findToFixTimeout) {
g->solved.timeout = true;
return;
}
ConstraintBase *c = &con;
if(c->group != g->h) continue;
if((c->type == Constraint::Type::POINTS_COINCIDENT && a == 0) ||
(c->type != Constraint::Type::POINTS_COINCIDENT && a == 1))
{
// Do the constraints in two passes: first everything but
// the point-coincident constraints, then only those
// constraints (so they appear last in the list).
continue;
}
param.ClearTags();
eq.Clear();
WriteEquationsExceptFor(c->h, g);
eq.ClearTags();
// It's a major speedup to solve the easy ones by substitution here,
// and that doesn't break anything.
if(!forceDofCheck) {
SolveBySubstitution();
}
WriteJacobian(0);
EvalJacobian();
int rank = CalculateRank();
if(rank == mat.m) {
// We fixed it by removing this constraint
bad->Add(&(c->h));
}
}
}
}
SolveResult System::Solve(Group *g, int *rank, int *dof, List<hConstraint> *bad,
bool andFindBad, bool andFindFree, bool forceDofCheck)
{
WriteEquationsExceptFor(Constraint::NO_CONSTRAINT, g);
bool rankOk;
/*
int x;
dbp("%d equations", eq.n);
for(x = 0; x < eq.n; x++) {
dbp(" %.3f = %s = 0", eq[x].e->Eval(), eq[x].e->Print());
}
dbp("%d parameters", param.n);
for(x = 0; x < param.n; x++) {
dbp(" param %08x at %.3f", param[x].h.v, param[x].val);
} */
// All params and equations are assigned to group zero.
param.ClearTags();
eq.ClearTags();
// Since we are suppressing dof calculation or allowing redundant, we
// can't / don't want to catch result of dof checking without substitution
if(g->suppressDofCalculation || g->allowRedundant || !forceDofCheck) {
SolveBySubstitution();
}
// Before solving the big system, see if we can find any equations that
// are soluble alone. This can be a huge speedup. We don't know whether
// the system is consistent yet, but if it isn't then we'll catch that
// later.
int alone = 1;
for(auto &e : eq) {
if(e.tag != 0)
continue;
hParam hp = e.e->ReferencedParams(&param);
if(hp == Expr::NO_PARAMS) continue;
if(hp == Expr::MULTIPLE_PARAMS) continue;
Param *p = param.FindById(hp);
if(p->tag != 0) continue; // let rank test catch inconsistency
e.tag = alone;
p->tag = alone;
WriteJacobian(alone);
if(!NewtonSolve(alone)) {
// We don't do the rank test, so let's arbitrarily return
// the DIDNT_CONVERGE result here.
rankOk = true;
// Failed to converge, bail out early
goto didnt_converge;
}
alone++;
}
// Now write the Jacobian for what's left, and do a rank test; that
// tells us if the system is inconsistently constrained.
if(!WriteJacobian(0)) {
return SolveResult::TOO_MANY_UNKNOWNS;
}
// Clear dof value in order to have indication when dof is actually not calculated
if(dof != NULL) *dof = -1;
// We are suppressing or allowing redundant, so we no need to catch unsolveable + redundant
rankOk = (!g->suppressDofCalculation && !g->allowRedundant) ? TestRank(dof) : true;
// And do the leftovers as one big system
if(!NewtonSolve(0)) {
goto didnt_converge;
}
// Here we are want to calculate dof even when redundant is allowed, so just handle suppressing
rankOk = (!g->suppressDofCalculation) ? TestRank(dof) : true;
if(!rankOk) {
if(andFindBad) FindWhichToRemoveToFixJacobian(g, bad, forceDofCheck);
} else {
MarkParamsFree(andFindFree);
}
// System solved correctly, so write the new values back in to the
// main parameter table.
for(auto &p : param) {
double val;
if(p.tag == VAR_SUBSTITUTED) {
val = p.substd->val;
} else {
val = p.val;
}
Param *pp = SK.GetParam(p.h);
pp->val = val;
pp->known = true;
pp->free = p.free;
}
return rankOk ? SolveResult::OKAY : SolveResult::REDUNDANT_OKAY;
didnt_converge:
SK.constraint.ClearTags();
// Not using range-for here because index is used in additional ways
for(size_t 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]->h.isFromConstraint()) continue;
hConstraint hc = mat.eq[i]->h.constraint();
ConstraintBase *c = SK.constraint.FindByIdNoOops(hc);
if(!c) continue;
// Don't double-show constraints that generated multiple
// unsatisfied equations
if(!c->tag) {
bad->Add(&(c->h));
c->tag = 1;
}
}
}
return rankOk ? SolveResult::DIDNT_CONVERGE : SolveResult::REDUNDANT_DIDNT_CONVERGE;
}
SolveResult System::SolveRank(Group *g, int *rank, int *dof, List<hConstraint> *bad,
bool andFindBad, bool andFindFree)
{
WriteEquationsExceptFor(Constraint::NO_CONSTRAINT, g);
// All params and equations are assigned to group zero.
param.ClearTags();
eq.ClearTags();
// Now write the Jacobian, and do a rank test; that
// tells us if the system is inconsistently constrained.
if(!WriteJacobian(0)) {
return SolveResult::TOO_MANY_UNKNOWNS;
}
bool rankOk = TestRank(dof);
if(!rankOk) {
// When we are testing with redundant allowed, we don't want to have additional info
// about redundants since this test is working only for single redundant constraint
if(!g->suppressDofCalculation && !g->allowRedundant) {
if(andFindBad) FindWhichToRemoveToFixJacobian(g, bad, true);
}
} else {
MarkParamsFree(andFindFree);
}
return rankOk ? SolveResult::OKAY : SolveResult::REDUNDANT_OKAY;
}
void System::Clear() {
entity.Clear();
param.Clear();
eq.Clear();
dragged.Clear();
mat.A.num.setZero();
mat.A.sym.setZero();
}
void System::MarkParamsFree(bool find) {
// If requested, find all the free (unbound) variables. This might be
// more than the number of degrees of freedom. Don't always do this,
// because the display would get annoying and it's slow.
for(auto &p : param) {
p.free = false;
if(find) {
if(p.tag == 0) {
p.tag = VAR_DOF_TEST;
WriteJacobian(0);
EvalJacobian();
int rank = CalculateRank();
if(rank == mat.m) {
p.free = true;
}
p.tag = 0;
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink