Rename the variables for the linear system to solve, for a bit more
clarity. [git-p4: depot-paths = "//depot/solvespace/": change = 1676]
This commit is contained in:
Jonathan Westhues
parent
b78b10ac1a
commit
7220f998fc
31
solvespace.h
31
solvespace.h
|
|
@ -86,24 +86,27 @@ public:
|
|||
static const int ASSUMED = 10000;
|
||||
static const int SUBSTITUTED = 10001;
|
||||
|
||||
// The Jacobian matrix
|
||||
// The system Jacobian matrix
|
||||
struct {
|
||||
hEquation eq[MAX_UNKNOWNS];
|
||||
// The corresponding equation for each row
|
||||
hEquation eq[MAX_UNKNOWNS];
|
||||
|
||||
// The corresponding parameter for each column
|
||||
hParam param[MAX_UNKNOWNS];
|
||||
bool bound[MAX_UNKNOWNS];
|
||||
|
||||
// We're solving AX = B
|
||||
int m, n;
|
||||
struct {
|
||||
Expr *sym[MAX_UNKNOWNS][MAX_UNKNOWNS];
|
||||
double num[MAX_UNKNOWNS][MAX_UNKNOWNS];
|
||||
} A;
|
||||
double X[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;
|
||||
} B;
|
||||
} mat;
|
||||
|
||||
bool Tol(double v);
|
||||
void GaussJordan(void);
|
||||
|
|
|
|||
153
system.cpp
153
system.cpp
|
|
@ -7,31 +7,31 @@ void System::WriteJacobian(int eqTag, int paramTag) {
|
|||
for(a = 0; a < param.n; a++) {
|
||||
Param *p = &(param.elem[a]);
|
||||
if(p->tag != paramTag) continue;
|
||||
J.param[j] = p->h;
|
||||
mat.param[j] = p->h;
|
||||
j++;
|
||||
}
|
||||
J.n = j;
|
||||
mat.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]);
|
||||
mat.eq[i] = eq.elem[i].h;
|
||||
mat.B.sym[i] = eq.elem[i].e;
|
||||
for(j = 0; j < mat.n; j++) {
|
||||
mat.A.sym[i][j] = e->e->PartialWrt(mat.param[j]);
|
||||
}
|
||||
i++;
|
||||
}
|
||||
J.m = i;
|
||||
mat.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();
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -43,18 +43,18 @@ bool System::Tol(double v) {
|
|||
void System::GaussJordan(void) {
|
||||
int i, j;
|
||||
|
||||
for(j = 0; j < J.n; j++) {
|
||||
J.bound[j] = false;
|
||||
for(j = 0; j < mat.n; j++) {
|
||||
mat.bound[j] = false;
|
||||
}
|
||||
|
||||
// Now eliminate.
|
||||
i = 0;
|
||||
for(j = 0; j < J.n; j++) {
|
||||
for(j = 0; j < mat.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]);
|
||||
for(ip = i; ip < mat.m; ip++) {
|
||||
double v = fabs(mat.A.num[ip][j]);
|
||||
if(v > max) {
|
||||
imax = ip;
|
||||
max = v;
|
||||
|
|
@ -63,96 +63,96 @@ void System::GaussJordan(void) {
|
|||
if(!Tol(max)) {
|
||||
// There's a usable pivot in this column. Swap it in:
|
||||
int js;
|
||||
for(js = j; js < J.n; js++) {
|
||||
for(js = j; js < mat.n; js++) {
|
||||
double temp;
|
||||
temp = J.num[imax][js];
|
||||
J.num[imax][js] = J.num[i][js];
|
||||
J.num[i][js] = temp;
|
||||
temp = mat.A.num[imax][js];
|
||||
mat.A.num[imax][js] = mat.A.num[i][js];
|
||||
mat.A.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;
|
||||
double v = mat.A.num[i][j];
|
||||
for(js = 0; js < mat.n; js++) {
|
||||
mat.A.num[i][js] /= v;
|
||||
}
|
||||
|
||||
// Eliminate this column from rows except this one.
|
||||
int is;
|
||||
for(is = 0; is < J.m; is++) {
|
||||
for(is = 0; is < mat.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];
|
||||
// We're trying to drive A[is][j] to zero. We know
|
||||
// that A[i][j] is 1, so we want to subtract off
|
||||
// A[is][j] times our present row.
|
||||
double v = mat.A.num[is][j];
|
||||
for(js = 0; js < mat.n; js++) {
|
||||
mat.A.num[is][js] -= v*mat.A.num[i][js];
|
||||
}
|
||||
J.num[is][j] = 0;
|
||||
mat.A.num[is][j] = 0;
|
||||
}
|
||||
|
||||
// And mark this as a bound variable.
|
||||
J.bound[j] = true;
|
||||
mat.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;
|
||||
if(i >= mat.m) break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool System::SolveLinearSystem(void) {
|
||||
if(J.m != J.n) oops();
|
||||
if(mat.m != mat.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++) {
|
||||
for(i = 0; i < mat.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) {
|
||||
for(ip = i; ip < mat.m; ip++) {
|
||||
if(fabs(mat.A.num[ip][i]) > max) {
|
||||
imax = ip;
|
||||
max = fabs(J.num[ip][i]);
|
||||
max = fabs(mat.A.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;
|
||||
for(jp = 0; jp < mat.n; jp++) {
|
||||
temp = mat.A.num[i][jp];
|
||||
mat.A.num[i][jp] = mat.A.num[imax][jp];
|
||||
mat.A.num[imax][jp] = temp;
|
||||
}
|
||||
temp = J.B.num[i];
|
||||
J.B.num[i] = J.B.num[imax];
|
||||
J.B.num[imax] = temp;
|
||||
temp = mat.B.num[i];
|
||||
mat.B.num[i] = mat.B.num[imax];
|
||||
mat.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(ip = i+1; ip < mat.m; ip++) {
|
||||
temp = mat.A.num[ip][i]/mat.A.num[i][i];
|
||||
|
||||
for(jp = 0; jp < J.n; jp++) {
|
||||
J.num[ip][jp] -= temp*(J.num[i][jp]);
|
||||
for(jp = 0; jp < mat.n; jp++) {
|
||||
mat.A.num[ip][jp] -= temp*(mat.A.num[i][jp]);
|
||||
}
|
||||
J.B.num[ip] -= temp*J.B.num[i];
|
||||
mat.B.num[ip] -= temp*mat.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;
|
||||
for(i = mat.m - 1; i >= 0; i--) {
|
||||
if(fabs(mat.A.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];
|
||||
temp = mat.B.num[i];
|
||||
for(j = mat.n - 1; j > i; j--) {
|
||||
temp -= mat.X[j]*mat.A.num[i][j];
|
||||
}
|
||||
J.X[i] = temp / J.num[i][i];
|
||||
mat.X[i] = temp / mat.A.num[i][i];
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
@ -160,17 +160,17 @@ bool System::SolveLinearSystem(void) {
|
|||
|
||||
bool System::NewtonSolve(int tag) {
|
||||
WriteJacobian(tag, tag);
|
||||
if(J.m != J.n) oops();
|
||||
if(mat.m != mat.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());
|
||||
for(i = 0; i < mat.m; i++) {
|
||||
mat.B.num[i] = (mat.B.sym[i])->Eval();
|
||||
dbp("mat.B.num[%d] = %.3f", i, mat.B.num[i]);
|
||||
dbp("mat.B.sym[%d] = %s", i, (mat.B.sym[i])->Print());
|
||||
}
|
||||
// And likewise for the Jacobian
|
||||
EvalJacobian();
|
||||
|
|
@ -179,19 +179,20 @@ bool System::NewtonSolve(int tag) {
|
|||
|
||||
// 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];
|
||||
for(i = 0; i < mat.m; i++) {
|
||||
dbp("mat.X[%d] = %.3f", i, mat.X[i]);
|
||||
dbp("modifying param %08x, now %.3f", mat.param[i],
|
||||
param.FindById(mat.param[i])->val);
|
||||
(param.FindById(mat.param[i]))->val -= mat.X[i];
|
||||
// XXX do this properly
|
||||
SS.GetParam(J.param[i])->val = (param.FindById(J.param[i]))->val;
|
||||
SS.GetParam(mat.param[i])->val =
|
||||
(param.FindById(mat.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])) {
|
||||
for(i = 0; i < mat.m; i++) {
|
||||
if(!Tol(mat.B.num[i])) {
|
||||
converged = false;
|
||||
break;
|
||||
}
|
||||
|
|
@ -218,23 +219,23 @@ bool System::Solve(void) {
|
|||
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]);
|
||||
for(i = 0; i < mat.m; i++) {
|
||||
for(j = 0; j < mat.n; j++) {
|
||||
dbp("A[%d][%d] = %.3f", i, j, mat.A.num[i][j]);
|
||||
}
|
||||
}
|
||||
|
||||
GaussJordan();
|
||||
|
||||
dbp("bound states:");
|
||||
for(j = 0; j < J.n; j++) {
|
||||
dbp(" param %08x: %d", J.param[j], J.bound[j]);
|
||||
for(j = 0; j < mat.n; j++) {
|
||||
dbp(" param %08x: %d", mat.param[j], mat.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;
|
||||
for(j = 0; j < mat.n; j++) {
|
||||
if(mat.bound[j]) continue;
|
||||
param.FindById(mat.param[j])->tag = ASSUMED;
|
||||
}
|
||||
|
||||
NewtonSolve(0);
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user