Ultra-rough beginnings of a solver. Write the constraint equations,
take the partial derivatives, and run the Newton's method. This seems to sort of work with a single distance constraint. [git-p4: depot-paths = "//depot/solvespace/": change = 1675]solver
Jonathan Westhues
parent
ed50632610
commit
b78b10ac1a
1
Makefile
1
Makefile
|
|
@ -20,6 +20,7 @@ SSOBJS = $(OBJDIR)\solvespace.obj \
|
|||
$(OBJDIR)\constraint.obj \
|
||||
$(OBJDIR)\drawconstraint.obj \
|
||||
$(OBJDIR)\file.obj \
|
||||
$(OBJDIR)\system.obj \
|
||||
|
||||
|
||||
LIBS = user32.lib gdi32.lib comctl32.lib advapi32.lib opengl32.lib glu32.lib
|
||||
|
|
|
|||
|
|
@ -29,13 +29,65 @@ void Constraint::MenuConstrain(int id) {
|
|||
return;
|
||||
}
|
||||
c.disp.offset = Vector::MakeFrom(50, 50, 50);
|
||||
c.exprA = Expr::FromString("1+3+2")->DeepCopyKeep();
|
||||
c.exprA = Expr::FromString("300")->DeepCopyKeep();
|
||||
AddConstraint(&c);
|
||||
break;
|
||||
|
||||
case GraphicsWindow::MNU_SOLVE_NOW:
|
||||
SS.Solve();
|
||||
return;
|
||||
|
||||
default: oops();
|
||||
}
|
||||
|
||||
SS.GW.ClearSelection();
|
||||
InvalidateGraphics();
|
||||
}
|
||||
|
||||
Expr *Constraint::Distance(hEntity hpa, hEntity hpb) {
|
||||
Entity *pa = SS.GetEntity(hpa);
|
||||
Entity *pb = SS.GetEntity(hpb);
|
||||
|
||||
if(!(pa->IsPoint() && pb->IsPoint())) oops();
|
||||
|
||||
if(pa->type == Entity::POINT_IN_2D &&
|
||||
pb->type == Entity::POINT_IN_2D &&
|
||||
pa->csys.v == pb->csys.v)
|
||||
{
|
||||
// A nice case; they are both in the same 2d csys, so I can write
|
||||
// the equation in terms of the basis vectors in that csys.
|
||||
Expr *du = Expr::FromParam(pa->param.h[0])->Minus(
|
||||
Expr::FromParam(pb->param.h[0]));
|
||||
Expr *dv = Expr::FromParam(pa->param.h[1])->Minus(
|
||||
Expr::FromParam(pb->param.h[1]));
|
||||
|
||||
return ((du->Square())->Plus(dv->Square()))->Sqrt();
|
||||
}
|
||||
Expr *ax, *ay, *az;
|
||||
Expr *bx, *by, *bz;
|
||||
pa->PointGetExprs(&ax, &ay, &az);
|
||||
pb->PointGetExprs(&bx, &by, &bz);
|
||||
|
||||
Expr *dx2 = (ax->Minus(bx))->Square();
|
||||
Expr *dy2 = (ay->Minus(by))->Square();
|
||||
Expr *dz2 = (az->Minus(bz))->Square();
|
||||
|
||||
return (dx2->Plus(dy2->Plus(dz2)))->Sqrt();
|
||||
}
|
||||
|
||||
void Constraint::AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index) {
|
||||
Equation eq;
|
||||
eq.e = expr;
|
||||
eq.h = h.equation(index);
|
||||
l->Add(&eq);
|
||||
}
|
||||
|
||||
void Constraint::Generate(IdList<Equation,hEquation> *l) {
|
||||
switch(type) {
|
||||
case PT_PT_DISTANCE:
|
||||
AddEq(l, Distance(ptA, ptB)->Minus(exprA), 0);
|
||||
break;
|
||||
|
||||
default: oops();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
54
entity.cpp
54
entity.cpp
|
|
@ -16,6 +16,33 @@ void Entity::Csys2dGetBasisVectors(Vector *u, Vector *v) {
|
|||
*v = quat.RotationV();
|
||||
}
|
||||
|
||||
void Entity::Csys2dGetBasisExprs(Expr **u, Expr **v) {
|
||||
Expr *a = Expr::FromParam(param.h[0]);
|
||||
Expr *b = Expr::FromParam(param.h[1]);
|
||||
Expr *c = Expr::FromParam(param.h[2]);
|
||||
Expr *d = Expr::FromParam(param.h[3]);
|
||||
|
||||
Expr *two = Expr::FromConstant(2);
|
||||
|
||||
u[0] = a->Square();
|
||||
u[0] = (u[0])->Plus(b->Square());
|
||||
u[0] = (u[0])->Minus(c->Square());
|
||||
u[0] = (u[0])->Minus(d->Square());
|
||||
u[1] = two->Times(a->Times(d));
|
||||
u[1] = (u[1])->Plus(two->Times(b->Times(c)));
|
||||
u[2] = two->Times(b->Times(d));
|
||||
u[2] = (u[2])->Minus(two->Times(a->Times(c)));
|
||||
|
||||
v[0] = two->Times(b->Times(c));
|
||||
v[0] = (v[0])->Minus(two->Times(a->Times(d)));
|
||||
v[1] = a->Square();
|
||||
v[1] = (v[1])->Minus(b->Square());
|
||||
v[1] = (v[1])->Plus(c->Square());
|
||||
v[1] = (v[1])->Minus(d->Square());
|
||||
v[2] = two->Times(a->Times(b));
|
||||
v[2] = (v[2])->Plus(two->Times(c->Times(d)));
|
||||
}
|
||||
|
||||
bool Entity::IsPoint(void) {
|
||||
switch(type) {
|
||||
case POINT_IN_3D:
|
||||
|
|
@ -76,6 +103,33 @@ Vector Entity::PointGetCoords(void) {
|
|||
return p;
|
||||
}
|
||||
|
||||
void Entity::PointGetExprs(Expr **x, Expr **y, Expr **z) {
|
||||
switch(type) {
|
||||
case POINT_IN_3D:
|
||||
*x = Expr::FromParam(param.h[0]);
|
||||
*y = Expr::FromParam(param.h[1]);
|
||||
*z = Expr::FromParam(param.h[2]);
|
||||
break;
|
||||
|
||||
case POINT_IN_2D: {
|
||||
Entity *c = SS.GetEntity(csys);
|
||||
Expr *u[3], *v[3];
|
||||
c->Csys2dGetBasisExprs(u, v);
|
||||
|
||||
*x = Expr::FromParam(param.h[0])->Times(u[0]);
|
||||
*x = (*x)->Plus(Expr::FromParam(param.h[1])->Times(v[0]));
|
||||
|
||||
*y = Expr::FromParam(param.h[0])->Times(u[1]);
|
||||
*y = (*y)->Plus(Expr::FromParam(param.h[1])->Times(v[1]));
|
||||
|
||||
*z = Expr::FromParam(param.h[0])->Times(u[2]);
|
||||
*z = (*z)->Plus(Expr::FromParam(param.h[1])->Times(v[2]));
|
||||
break;
|
||||
}
|
||||
default: oops();
|
||||
}
|
||||
}
|
||||
|
||||
void Entity::LineDrawOrGetDistance(Vector a, Vector b) {
|
||||
if(dogd.drawing) {
|
||||
glBegin(GL_LINE_STRIP);
|
||||
|
|
|
|||
|
|
@ -75,7 +75,8 @@ const GraphicsWindow::MenuEntry GraphicsWindow::menu[] = {
|
|||
{ 1, "S&ymmetric\tShift+Y", 0, 'Y'|S, NULL },
|
||||
{ 1, NULL, 0, NULL },
|
||||
{ 1, "Sym&bolic Equation\tShift+B", 0, 'B'|S, NULL },
|
||||
|
||||
{ 1, NULL, 0, NULL },
|
||||
{ 1, "Solve Once Now\tSpace", MNU_SOLVE_NOW, ' ', mCon },
|
||||
|
||||
{ 0, "&Help", 0, NULL },
|
||||
{ 1, "&About\t", 0, NULL },
|
||||
|
|
|
|||
34
sketch.h
34
sketch.h
|
|
@ -53,13 +53,29 @@ public:
|
|||
int tag;
|
||||
hGroup h;
|
||||
|
||||
int solveOrder;
|
||||
bool solved;
|
||||
|
||||
NameStr name;
|
||||
|
||||
char *DescriptionString(void);
|
||||
};
|
||||
|
||||
|
||||
class EntityId {
|
||||
DWORD v; // entity ID, starting from 0
|
||||
};
|
||||
class EntityMap {
|
||||
int tag;
|
||||
|
||||
EntityId h;
|
||||
hEntity input;
|
||||
int copyNumber;
|
||||
// (input, copyNumber) gets mapped to ((Request)xxx).entity(h.v)
|
||||
};
|
||||
|
||||
// A user request for some primitive or derived operation; for example a
|
||||
// line, or a
|
||||
// line, or a step and repeat.
|
||||
class Request {
|
||||
public:
|
||||
// Some predefined requests, that are present in every sketch.
|
||||
|
|
@ -83,6 +99,13 @@ public:
|
|||
|
||||
NameStr name;
|
||||
|
||||
// When a request generates entities from entities, and the source
|
||||
// entities may have come from multiple requests, it's necessary to
|
||||
// remap the entity ID so that it's still unique. We do this with a
|
||||
// mapping list.
|
||||
IdList<EntityId,EntityMap> remap;
|
||||
hEntity Remap(hEntity in, int copyNumber);
|
||||
|
||||
hParam AddParam(IdList<Param,hParam> *param, hParam hp);
|
||||
void Generate(IdList<Entity,hEntity> *entity, IdList<Param,hParam> *param);
|
||||
|
||||
|
|
@ -116,6 +139,7 @@ public:
|
|||
|
||||
// Applies only for a CSYS_2D type
|
||||
void Csys2dGetBasisVectors(Vector *u, Vector *v);
|
||||
void Csys2dGetBasisExprs(Expr **u, Expr **v);
|
||||
|
||||
bool IsPoint(void);
|
||||
// Applies for any of the point types
|
||||
|
|
@ -166,6 +190,8 @@ inline hRequest hParam::request(void)
|
|||
class hConstraint {
|
||||
public:
|
||||
DWORD v;
|
||||
|
||||
hEquation equation(int i);
|
||||
};
|
||||
|
||||
class Constraint {
|
||||
|
|
@ -213,6 +239,9 @@ public:
|
|||
bool HasLabel(void);
|
||||
|
||||
void Generate(IdList<Equation,hEquation> *l);
|
||||
// Some helpers when generating symbolic constraint equations
|
||||
void AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index);
|
||||
static Expr *Distance(hEntity pa, hEntity pb);
|
||||
};
|
||||
|
||||
class hEquation {
|
||||
|
|
@ -228,5 +257,8 @@ public:
|
|||
Expr *e;
|
||||
};
|
||||
|
||||
inline hEquation hConstraint::equation(int i)
|
||||
{ hEquation r; r.v = (v << 16) | i; return r; }
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ void SolveSpace::GenerateAll(void) {
|
|||
}
|
||||
}
|
||||
|
||||
prev.Clear();
|
||||
ForceReferences();
|
||||
}
|
||||
|
||||
|
|
@ -94,7 +95,82 @@ void SolveSpace::ForceReferences(void) {
|
|||
}
|
||||
}
|
||||
|
||||
bool SolveSpace::SolveGroup(hGroup hg) {
|
||||
int i;
|
||||
if(hg.v == Group::HGROUP_REFERENCES.v) {
|
||||
// Special case; mark everything in the references known.
|
||||
for(i = 0; i < param.n; i++) {
|
||||
Param *p = &(param.elem[i]);
|
||||
Request *r = GetRequest(p->h.request());
|
||||
if(r->group.v == hg.v) p->known = true;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Clear out the system to be solved.
|
||||
sys.entity.Clear();
|
||||
sys.param.Clear();
|
||||
sys.eq.Clear();
|
||||
// And generate all the params for requests in this group
|
||||
for(i = 0; i < request.n; i++) {
|
||||
Request *r = &(request.elem[i]);
|
||||
if(r->group.v != hg.v) continue;
|
||||
|
||||
r->Generate(&(sys.entity), &(sys.param));
|
||||
}
|
||||
// Set the initial guesses for all the params
|
||||
for(i = 0; i < sys.param.n; i++) {
|
||||
Param *p = &(sys.param.elem[i]);
|
||||
p->known = false;
|
||||
p->val = GetParam(p->h)->val;
|
||||
}
|
||||
// And generate all the equations from constraints in this group
|
||||
for(i = 0; i < constraint.n; i++) {
|
||||
Constraint *c = &(constraint.elem[i]);
|
||||
if(c->group.v != hg.v) continue;
|
||||
|
||||
c->Generate(&(sys.eq));
|
||||
}
|
||||
|
||||
return sys.Solve();
|
||||
}
|
||||
|
||||
bool SolveSpace::SolveWorker(void) {
|
||||
bool allSolved = true;
|
||||
|
||||
int i;
|
||||
for(i = 0; i < group.n; i++) {
|
||||
Group *g = &(group.elem[i]);
|
||||
if(g->solved) continue;
|
||||
|
||||
allSolved = false;
|
||||
dbp("try solve group %s", g->DescriptionString());
|
||||
if(SolveGroup(g->h)) {
|
||||
g->solved = true;
|
||||
// So this one worked; let's see if we can go any further.
|
||||
if(SolveWorker()) {
|
||||
// So everything worked; we're done.
|
||||
return true;
|
||||
} else {
|
||||
// Didn't work, so undo this choice and give up
|
||||
g->solved = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// If we got here, then either everything failed, so we're stuck, or
|
||||
// everything was already solved, so we're done.
|
||||
return allSolved;
|
||||
}
|
||||
|
||||
void SolveSpace::Solve(void) {
|
||||
int i;
|
||||
for(i = 0; i < group.n; i++) {
|
||||
group.elem[i].solved = false;
|
||||
}
|
||||
SolveWorker();
|
||||
|
||||
InvalidateGraphics();
|
||||
}
|
||||
|
||||
void SolveSpace::MenuFile(int id) {
|
||||
|
|
|
|||
50
solvespace.h
50
solvespace.h
|
|
@ -73,6 +73,49 @@ void MakeMatrix(double *mat, double a11, double a12, double a13, double a14,
|
|||
double a31, double a32, double a33, double a34,
|
||||
double a41, double a42, double a43, double a44);
|
||||
|
||||
class System {
|
||||
public:
|
||||
#define MAX_UNKNOWNS 200
|
||||
|
||||
IdList<Entity,hEntity> entity;
|
||||
IdList<Param,hParam> param;
|
||||
IdList<Equation,hEquation> eq;
|
||||
|
||||
// In general, the tag indicates the subsys that a variable/equation
|
||||
// has been assigned to; these are exceptions.
|
||||
static const int ASSUMED = 10000;
|
||||
static const int SUBSTITUTED = 10001;
|
||||
|
||||
// The Jacobian matrix
|
||||
struct {
|
||||
hEquation eq[MAX_UNKNOWNS];
|
||||
struct {
|
||||
Expr *sym[MAX_UNKNOWNS];
|
||||
double num[MAX_UNKNOWNS];
|
||||
} B;
|
||||
|
||||
hParam param[MAX_UNKNOWNS];
|
||||
bool bound[MAX_UNKNOWNS];
|
||||
|
||||
Expr *sym[MAX_UNKNOWNS][MAX_UNKNOWNS];
|
||||
double num[MAX_UNKNOWNS][MAX_UNKNOWNS];
|
||||
|
||||
double X[MAX_UNKNOWNS];
|
||||
|
||||
int m, n;
|
||||
} J;
|
||||
|
||||
bool Tol(double v);
|
||||
void GaussJordan(void);
|
||||
bool SolveLinearSystem(void);
|
||||
|
||||
void WriteJacobian(int eqTag, int paramTag);
|
||||
void EvalJacobian(void);
|
||||
|
||||
bool NewtonSolve(int tag);
|
||||
bool Solve(void);
|
||||
};
|
||||
|
||||
|
||||
class SolveSpace {
|
||||
public:
|
||||
|
|
@ -93,6 +136,7 @@ public:
|
|||
inline Request *GetRequest(hRequest h) { return request.FindById(h); }
|
||||
inline Entity *GetEntity (hEntity h) { return entity. FindById(h); }
|
||||
inline Param *GetParam (hParam h) { return param. FindById(h); }
|
||||
inline Group *GetGroup (hGroup h) { return group. FindById(h); }
|
||||
|
||||
hGroup activeGroup;
|
||||
|
||||
|
|
@ -102,11 +146,17 @@ public:
|
|||
void ForceReferences(void);
|
||||
|
||||
void Init(void);
|
||||
|
||||
bool SolveGroup(hGroup hg);
|
||||
bool SolveWorker(void);
|
||||
void Solve(void);
|
||||
|
||||
static void MenuFile(int id);
|
||||
bool SaveToFile(char *filename);
|
||||
bool LoadFromFile(char *filename);
|
||||
|
||||
// The system to be solved.
|
||||
System sys;
|
||||
};
|
||||
|
||||
extern SolveSpace SS;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,244 @@
|
|||
#include "solvespace.h"
|
||||
|
||||
void System::WriteJacobian(int eqTag, int paramTag) {
|
||||
int a, i, j;
|
||||
|
||||
j = 0;
|
||||
for(a = 0; a < param.n; a++) {
|
||||
Param *p = &(param.elem[a]);
|
||||
if(p->tag != paramTag) continue;
|
||||
J.param[j] = p->h;
|
||||
j++;
|
||||
}
|
||||
J.n = j;
|
||||
|
||||
i = 0;
|
||||
for(a = 0; a < eq.n; a++) {
|
||||
Equation *e = &(eq.elem[a]);
|
||||
if(e->tag != eqTag) continue;
|
||||
|
||||
J.eq[i] = eq.elem[i].h;
|
||||
J.B.sym[i] = eq.elem[i].e;
|
||||
for(j = 0; j < J.n; j++) {
|
||||
J.sym[i][j] = e->e->PartialWrt(J.param[j]);
|
||||
}
|
||||
i++;
|
||||
}
|
||||
J.m = i;
|
||||
}
|
||||
|
||||
void System::EvalJacobian(void) {
|
||||
int i, j;
|
||||
for(i = 0; i < J.m; i++) {
|
||||
for(j = 0; j < J.n; j++) {
|
||||
J.num[i][j] = (J.sym[i][j])->Eval();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool System::Tol(double v) {
|
||||
return (fabs(v) < 0.01);
|
||||
}
|
||||
|
||||
void System::GaussJordan(void) {
|
||||
int i, j;
|
||||
|
||||
for(j = 0; j < J.n; j++) {
|
||||
J.bound[j] = false;
|
||||
}
|
||||
|
||||
// Now eliminate.
|
||||
i = 0;
|
||||
for(j = 0; j < J.n; j++) {
|
||||
// First, seek a pivot in our column.
|
||||
int ip, imax;
|
||||
double max = 0;
|
||||
for(ip = i; ip < J.m; ip++) {
|
||||
double v = fabs(J.num[ip][j]);
|
||||
if(v > max) {
|
||||
imax = ip;
|
||||
max = v;
|
||||
}
|
||||
}
|
||||
if(!Tol(max)) {
|
||||
// There's a usable pivot in this column. Swap it in:
|
||||
int js;
|
||||
for(js = j; js < J.n; js++) {
|
||||
double temp;
|
||||
temp = J.num[imax][js];
|
||||
J.num[imax][js] = J.num[i][js];
|
||||
J.num[i][js] = temp;
|
||||
}
|
||||
|
||||
// Get a 1 as the leading entry in the row we're working on.
|
||||
double v = J.num[i][j];
|
||||
for(js = 0; js < J.n; js++) {
|
||||
J.num[i][js] /= v;
|
||||
}
|
||||
|
||||
// Eliminate this column from rows except this one.
|
||||
int is;
|
||||
for(is = 0; is < J.m; is++) {
|
||||
if(is == i) continue;
|
||||
|
||||
// We're trying to drive J.num[is][j] to zero. We know
|
||||
// that J.num[i][j] is 1, so we want to subtract off
|
||||
// J.num[is][j] times our present row.
|
||||
double v = J.num[is][j];
|
||||
for(js = 0; js < J.n; js++) {
|
||||
J.num[is][js] -= v*J.num[i][js];
|
||||
}
|
||||
J.num[is][j] = 0;
|
||||
}
|
||||
|
||||
// And mark this as a bound variable.
|
||||
J.bound[j] = true;
|
||||
|
||||
// Move on to the next row, since we just used this one to
|
||||
// eliminate from column j.
|
||||
i++;
|
||||
if(i >= J.m) break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool System::SolveLinearSystem(void) {
|
||||
if(J.m != J.n) oops();
|
||||
// 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;
|
||||
double max, temp;
|
||||
|
||||
for(i = 0; i < J.m; 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 < J.m; ip++) {
|
||||
if(fabs(J.num[ip][i]) > max) {
|
||||
imax = ip;
|
||||
max = fabs(J.num[ip][i]);
|
||||
}
|
||||
}
|
||||
if(fabs(max) < 1e-12) return false;
|
||||
|
||||
// Swap row imax with row i
|
||||
for(jp = 0; jp < J.n; jp++) {
|
||||
temp = J.num[i][jp];
|
||||
J.num[i][jp] = J.num[imax][jp];
|
||||
J.num[imax][jp] = temp;
|
||||
}
|
||||
temp = J.B.num[i];
|
||||
J.B.num[i] = J.B.num[imax];
|
||||
J.B.num[imax] = temp;
|
||||
|
||||
// For rows i+1 and greater, eliminate the term in column i.
|
||||
for(ip = i+1; ip < J.m; ip++) {
|
||||
temp = J.num[ip][i]/J.num[i][i];
|
||||
|
||||
for(jp = 0; jp < J.n; jp++) {
|
||||
J.num[ip][jp] -= temp*(J.num[i][jp]);
|
||||
}
|
||||
J.B.num[ip] -= temp*J.B.num[i];
|
||||
}
|
||||
}
|
||||
|
||||
// We've put the matrix in upper triangular form, so at this point we
|
||||
// can solve by back-substitution.
|
||||
for(i = J.m - 1; i >= 0; i--) {
|
||||
if(fabs(J.num[i][i]) < 1e-10) return false;
|
||||
|
||||
temp = J.B.num[i];
|
||||
for(j = J.n - 1; j > i; j--) {
|
||||
temp -= J.X[j]*J.num[i][j];
|
||||
}
|
||||
J.X[i] = temp / J.num[i][i];
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool System::NewtonSolve(int tag) {
|
||||
WriteJacobian(tag, tag);
|
||||
if(J.m != J.n) oops();
|
||||
|
||||
int iter = 0;
|
||||
bool converged = false;
|
||||
int i;
|
||||
do {
|
||||
// Evaluate the functions numerically
|
||||
for(i = 0; i < J.m; i++) {
|
||||
J.B.num[i] = (J.B.sym[i])->Eval();
|
||||
dbp("J.B.num[%d] = %.3f", i, J.B.num[i]);
|
||||
dbp("J.B.sym[%d] = %s", i, (J.B.sym[i])->Print());
|
||||
}
|
||||
// And likewise for the Jacobian
|
||||
EvalJacobian();
|
||||
|
||||
if(!SolveLinearSystem()) break;
|
||||
|
||||
// Take the Newton step;
|
||||
// J(x_n) (x_{n+1} - x_n) = 0 - F(x_n)
|
||||
for(i = 0; i < J.m; i++) {
|
||||
dbp("J.X[%d] = %.3f", i, J.X[i]);
|
||||
dbp("modifying param %08x, now %.3f", J.param[i],
|
||||
param.FindById(J.param[i])->val);
|
||||
(param.FindById(J.param[i]))->val -= J.X[i];
|
||||
// XXX do this properly
|
||||
SS.GetParam(J.param[i])->val = (param.FindById(J.param[i]))->val;
|
||||
}
|
||||
|
||||
// XXX re-evaluate functions before checking convergence
|
||||
converged = true;
|
||||
for(i = 0; i < J.m; i++) {
|
||||
if(!Tol(J.B.num[i])) {
|
||||
converged = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} while(iter++ < 50 && !converged);
|
||||
|
||||
if(converged) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool System::Solve(void) {
|
||||
int i, j;
|
||||
dbp("%d equations", eq.n);
|
||||
for(i = 0; i < eq.n; i++) {
|
||||
dbp(" %s = 0", eq.elem[i].e->Print());
|
||||
}
|
||||
|
||||
param.ClearTags();
|
||||
eq.ClearTags();
|
||||
|
||||
WriteJacobian(0, 0);
|
||||
EvalJacobian();
|
||||
|
||||
for(i = 0; i < J.m; i++) {
|
||||
for(j = 0; j < J.n; j++) {
|
||||
dbp("J[%d][%d] = %.3f", i, j, J.num[i][j]);
|
||||
}
|
||||
}
|
||||
|
||||
GaussJordan();
|
||||
|
||||
dbp("bound states:");
|
||||
for(j = 0; j < J.n; j++) {
|
||||
dbp(" param %08x: %d", J.param[j], J.bound[j]);
|
||||
}
|
||||
|
||||
// Fix any still-free variables wherever they are now.
|
||||
for(j = 0; j < J.n; j++) {
|
||||
if(J.bound[j]) continue;
|
||||
param.FindById(J.param[j])->tag = ASSUMED;
|
||||
}
|
||||
|
||||
NewtonSolve(0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue