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
27
solvespace.h
27
solvespace.h
|
|
@ -86,24 +86,27 @@ public:
|
||||||
static const int ASSUMED = 10000;
|
static const int ASSUMED = 10000;
|
||||||
static const int SUBSTITUTED = 10001;
|
static const int SUBSTITUTED = 10001;
|
||||||
|
|
||||||
// The Jacobian matrix
|
// The system Jacobian matrix
|
||||||
struct {
|
struct {
|
||||||
|
// The corresponding equation for each row
|
||||||
hEquation eq[MAX_UNKNOWNS];
|
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 {
|
struct {
|
||||||
Expr *sym[MAX_UNKNOWNS];
|
Expr *sym[MAX_UNKNOWNS];
|
||||||
double num[MAX_UNKNOWNS];
|
double num[MAX_UNKNOWNS];
|
||||||
} B;
|
} B;
|
||||||
|
} mat;
|
||||||
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);
|
bool Tol(double v);
|
||||||
void GaussJordan(void);
|
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++) {
|
for(a = 0; a < param.n; a++) {
|
||||||
Param *p = &(param.elem[a]);
|
Param *p = &(param.elem[a]);
|
||||||
if(p->tag != paramTag) continue;
|
if(p->tag != paramTag) continue;
|
||||||
J.param[j] = p->h;
|
mat.param[j] = p->h;
|
||||||
j++;
|
j++;
|
||||||
}
|
}
|
||||||
J.n = j;
|
mat.n = j;
|
||||||
|
|
||||||
i = 0;
|
i = 0;
|
||||||
for(a = 0; a < eq.n; a++) {
|
for(a = 0; a < eq.n; a++) {
|
||||||
Equation *e = &(eq.elem[a]);
|
Equation *e = &(eq.elem[a]);
|
||||||
if(e->tag != eqTag) continue;
|
if(e->tag != eqTag) continue;
|
||||||
|
|
||||||
J.eq[i] = eq.elem[i].h;
|
mat.eq[i] = eq.elem[i].h;
|
||||||
J.B.sym[i] = eq.elem[i].e;
|
mat.B.sym[i] = eq.elem[i].e;
|
||||||
for(j = 0; j < J.n; j++) {
|
for(j = 0; j < mat.n; j++) {
|
||||||
J.sym[i][j] = e->e->PartialWrt(J.param[j]);
|
mat.A.sym[i][j] = e->e->PartialWrt(mat.param[j]);
|
||||||
}
|
}
|
||||||
i++;
|
i++;
|
||||||
}
|
}
|
||||||
J.m = i;
|
mat.m = i;
|
||||||
}
|
}
|
||||||
|
|
||||||
void System::EvalJacobian(void) {
|
void System::EvalJacobian(void) {
|
||||||
int i, j;
|
int i, j;
|
||||||
for(i = 0; i < J.m; i++) {
|
for(i = 0; i < mat.m; i++) {
|
||||||
for(j = 0; j < J.n; j++) {
|
for(j = 0; j < mat.n; j++) {
|
||||||
J.num[i][j] = (J.sym[i][j])->Eval();
|
mat.A.num[i][j] = (mat.A.sym[i][j])->Eval();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -43,18 +43,18 @@ bool System::Tol(double v) {
|
||||||
void System::GaussJordan(void) {
|
void System::GaussJordan(void) {
|
||||||
int i, j;
|
int i, j;
|
||||||
|
|
||||||
for(j = 0; j < J.n; j++) {
|
for(j = 0; j < mat.n; j++) {
|
||||||
J.bound[j] = false;
|
mat.bound[j] = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Now eliminate.
|
// Now eliminate.
|
||||||
i = 0;
|
i = 0;
|
||||||
for(j = 0; j < J.n; j++) {
|
for(j = 0; j < mat.n; j++) {
|
||||||
// First, seek a pivot in our column.
|
// First, seek a pivot in our column.
|
||||||
int ip, imax;
|
int ip, imax;
|
||||||
double max = 0;
|
double max = 0;
|
||||||
for(ip = i; ip < J.m; ip++) {
|
for(ip = i; ip < mat.m; ip++) {
|
||||||
double v = fabs(J.num[ip][j]);
|
double v = fabs(mat.A.num[ip][j]);
|
||||||
if(v > max) {
|
if(v > max) {
|
||||||
imax = ip;
|
imax = ip;
|
||||||
max = v;
|
max = v;
|
||||||
|
|
@ -63,96 +63,96 @@ void System::GaussJordan(void) {
|
||||||
if(!Tol(max)) {
|
if(!Tol(max)) {
|
||||||
// There's a usable pivot in this column. Swap it in:
|
// There's a usable pivot in this column. Swap it in:
|
||||||
int js;
|
int js;
|
||||||
for(js = j; js < J.n; js++) {
|
for(js = j; js < mat.n; js++) {
|
||||||
double temp;
|
double temp;
|
||||||
temp = J.num[imax][js];
|
temp = mat.A.num[imax][js];
|
||||||
J.num[imax][js] = J.num[i][js];
|
mat.A.num[imax][js] = mat.A.num[i][js];
|
||||||
J.num[i][js] = temp;
|
mat.A.num[i][js] = temp;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Get a 1 as the leading entry in the row we're working on.
|
// Get a 1 as the leading entry in the row we're working on.
|
||||||
double v = J.num[i][j];
|
double v = mat.A.num[i][j];
|
||||||
for(js = 0; js < J.n; js++) {
|
for(js = 0; js < mat.n; js++) {
|
||||||
J.num[i][js] /= v;
|
mat.A.num[i][js] /= v;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Eliminate this column from rows except this one.
|
// Eliminate this column from rows except this one.
|
||||||
int is;
|
int is;
|
||||||
for(is = 0; is < J.m; is++) {
|
for(is = 0; is < mat.m; is++) {
|
||||||
if(is == i) continue;
|
if(is == i) continue;
|
||||||
|
|
||||||
// We're trying to drive J.num[is][j] to zero. We know
|
// We're trying to drive A[is][j] to zero. We know
|
||||||
// that J.num[i][j] is 1, so we want to subtract off
|
// that A[i][j] is 1, so we want to subtract off
|
||||||
// J.num[is][j] times our present row.
|
// A[is][j] times our present row.
|
||||||
double v = J.num[is][j];
|
double v = mat.A.num[is][j];
|
||||||
for(js = 0; js < J.n; js++) {
|
for(js = 0; js < mat.n; js++) {
|
||||||
J.num[is][js] -= v*J.num[i][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.
|
// 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
|
// Move on to the next row, since we just used this one to
|
||||||
// eliminate from column j.
|
// eliminate from column j.
|
||||||
i++;
|
i++;
|
||||||
if(i >= J.m) break;
|
if(i >= mat.m) break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool System::SolveLinearSystem(void) {
|
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
|
// Gaussian elimination, with partial pivoting. It's an error if the
|
||||||
// matrix is singular, because that means two constraints are
|
// matrix is singular, because that means two constraints are
|
||||||
// equivalent.
|
// equivalent.
|
||||||
int i, j, ip, jp, imax;
|
int i, j, ip, jp, imax;
|
||||||
double max, temp;
|
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
|
// 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).
|
// greater. First, find a pivot (between rows i and N-1).
|
||||||
max = 0;
|
max = 0;
|
||||||
for(ip = i; ip < J.m; ip++) {
|
for(ip = i; ip < mat.m; ip++) {
|
||||||
if(fabs(J.num[ip][i]) > max) {
|
if(fabs(mat.A.num[ip][i]) > max) {
|
||||||
imax = ip;
|
imax = ip;
|
||||||
max = fabs(J.num[ip][i]);
|
max = fabs(mat.A.num[ip][i]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if(fabs(max) < 1e-12) return false;
|
if(fabs(max) < 1e-12) return false;
|
||||||
|
|
||||||
// Swap row imax with row i
|
// Swap row imax with row i
|
||||||
for(jp = 0; jp < J.n; jp++) {
|
for(jp = 0; jp < mat.n; jp++) {
|
||||||
temp = J.num[i][jp];
|
temp = mat.A.num[i][jp];
|
||||||
J.num[i][jp] = J.num[imax][jp];
|
mat.A.num[i][jp] = mat.A.num[imax][jp];
|
||||||
J.num[imax][jp] = temp;
|
mat.A.num[imax][jp] = temp;
|
||||||
}
|
}
|
||||||
temp = J.B.num[i];
|
temp = mat.B.num[i];
|
||||||
J.B.num[i] = J.B.num[imax];
|
mat.B.num[i] = mat.B.num[imax];
|
||||||
J.B.num[imax] = temp;
|
mat.B.num[imax] = temp;
|
||||||
|
|
||||||
// For rows i+1 and greater, eliminate the term in column i.
|
// For rows i+1 and greater, eliminate the term in column i.
|
||||||
for(ip = i+1; ip < J.m; ip++) {
|
for(ip = i+1; ip < mat.m; ip++) {
|
||||||
temp = J.num[ip][i]/J.num[i][i];
|
temp = mat.A.num[ip][i]/mat.A.num[i][i];
|
||||||
|
|
||||||
for(jp = 0; jp < J.n; jp++) {
|
for(jp = 0; jp < mat.n; jp++) {
|
||||||
J.num[ip][jp] -= temp*(J.num[i][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
|
// We've put the matrix in upper triangular form, so at this point we
|
||||||
// can solve by back-substitution.
|
// can solve by back-substitution.
|
||||||
for(i = J.m - 1; i >= 0; i--) {
|
for(i = mat.m - 1; i >= 0; i--) {
|
||||||
if(fabs(J.num[i][i]) < 1e-10) return false;
|
if(fabs(mat.A.num[i][i]) < 1e-10) return false;
|
||||||
|
|
||||||
temp = J.B.num[i];
|
temp = mat.B.num[i];
|
||||||
for(j = J.n - 1; j > i; j--) {
|
for(j = mat.n - 1; j > i; j--) {
|
||||||
temp -= J.X[j]*J.num[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;
|
return true;
|
||||||
|
|
@ -160,17 +160,17 @@ bool System::SolveLinearSystem(void) {
|
||||||
|
|
||||||
bool System::NewtonSolve(int tag) {
|
bool System::NewtonSolve(int tag) {
|
||||||
WriteJacobian(tag, tag);
|
WriteJacobian(tag, tag);
|
||||||
if(J.m != J.n) oops();
|
if(mat.m != mat.n) oops();
|
||||||
|
|
||||||
int iter = 0;
|
int iter = 0;
|
||||||
bool converged = false;
|
bool converged = false;
|
||||||
int i;
|
int i;
|
||||||
do {
|
do {
|
||||||
// Evaluate the functions numerically
|
// Evaluate the functions numerically
|
||||||
for(i = 0; i < J.m; i++) {
|
for(i = 0; i < mat.m; i++) {
|
||||||
J.B.num[i] = (J.B.sym[i])->Eval();
|
mat.B.num[i] = (mat.B.sym[i])->Eval();
|
||||||
dbp("J.B.num[%d] = %.3f", i, J.B.num[i]);
|
dbp("mat.B.num[%d] = %.3f", i, mat.B.num[i]);
|
||||||
dbp("J.B.sym[%d] = %s", i, (J.B.sym[i])->Print());
|
dbp("mat.B.sym[%d] = %s", i, (mat.B.sym[i])->Print());
|
||||||
}
|
}
|
||||||
// And likewise for the Jacobian
|
// And likewise for the Jacobian
|
||||||
EvalJacobian();
|
EvalJacobian();
|
||||||
|
|
@ -179,19 +179,20 @@ bool System::NewtonSolve(int tag) {
|
||||||
|
|
||||||
// Take the Newton step;
|
// Take the Newton step;
|
||||||
// J(x_n) (x_{n+1} - x_n) = 0 - F(x_n)
|
// J(x_n) (x_{n+1} - x_n) = 0 - F(x_n)
|
||||||
for(i = 0; i < J.m; i++) {
|
for(i = 0; i < mat.m; i++) {
|
||||||
dbp("J.X[%d] = %.3f", i, J.X[i]);
|
dbp("mat.X[%d] = %.3f", i, mat.X[i]);
|
||||||
dbp("modifying param %08x, now %.3f", J.param[i],
|
dbp("modifying param %08x, now %.3f", mat.param[i],
|
||||||
param.FindById(J.param[i])->val);
|
param.FindById(mat.param[i])->val);
|
||||||
(param.FindById(J.param[i]))->val -= J.X[i];
|
(param.FindById(mat.param[i]))->val -= mat.X[i];
|
||||||
// XXX do this properly
|
// 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
|
// XXX re-evaluate functions before checking convergence
|
||||||
converged = true;
|
converged = true;
|
||||||
for(i = 0; i < J.m; i++) {
|
for(i = 0; i < mat.m; i++) {
|
||||||
if(!Tol(J.B.num[i])) {
|
if(!Tol(mat.B.num[i])) {
|
||||||
converged = false;
|
converged = false;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
@ -218,23 +219,23 @@ bool System::Solve(void) {
|
||||||
WriteJacobian(0, 0);
|
WriteJacobian(0, 0);
|
||||||
EvalJacobian();
|
EvalJacobian();
|
||||||
|
|
||||||
for(i = 0; i < J.m; i++) {
|
for(i = 0; i < mat.m; i++) {
|
||||||
for(j = 0; j < J.n; j++) {
|
for(j = 0; j < mat.n; j++) {
|
||||||
dbp("J[%d][%d] = %.3f", i, j, J.num[i][j]);
|
dbp("A[%d][%d] = %.3f", i, j, mat.A.num[i][j]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
GaussJordan();
|
GaussJordan();
|
||||||
|
|
||||||
dbp("bound states:");
|
dbp("bound states:");
|
||||||
for(j = 0; j < J.n; j++) {
|
for(j = 0; j < mat.n; j++) {
|
||||||
dbp(" param %08x: %d", J.param[j], J.bound[j]);
|
dbp(" param %08x: %d", mat.param[j], mat.bound[j]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Fix any still-free variables wherever they are now.
|
// Fix any still-free variables wherever they are now.
|
||||||
for(j = 0; j < J.n; j++) {
|
for(j = 0; j < mat.n; j++) {
|
||||||
if(J.bound[j]) continue;
|
if(mat.bound[j]) continue;
|
||||||
param.FindById(J.param[j])->tag = ASSUMED;
|
param.FindById(mat.param[j])->tag = ASSUMED;
|
||||||
}
|
}
|
||||||
|
|
||||||
NewtonSolve(0);
|
NewtonSolve(0);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user