diff --git a/solvespace.h b/solvespace.h index 5211f1c..96cda29 100644 --- a/solvespace.h +++ b/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); diff --git a/system.cpp b/system.cpp index e49a765..c4cc66e 100644 --- a/system.cpp +++ b/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);