becoming really crufty old code =)
-// solves Ax = B by Gauss-Jordan elimination, where A is a 3x3 matrix,
-// x is a column vector of length 3 and B is a row vector of length 3.
+// solves Ax = B by Gauss-Jordan elimination, where A is an NxN matrix,
+// x is a column vector of length N and B is a row vector of length N.
// Destroys its input in the process.
// Destroys its input in the process.
-static void solve3x3(double *A, double *x, double *B)
+template<int N>
+static void solve_matrix(double *A, double *x, double *B)
- // row 1 -= row 0 * (a1/a0)
- {
- double f = A[1] / A[0];
- A[1] = 0.0;
- A[4] -= A[3] * f;
- A[7] -= A[6] * f;
-
- B[1] -= B[0] * f;
- }
-
- // row 2 -= row 0 * (a2/a0)
- {
- double f = A[2] / A[0];
- A[2] = 0.0;
- A[5] -= A[3] * f;
- A[8] -= A[6] * f;
-
- B[2] -= B[0] * f;
- }
-
- // row 2 -= row 1 * (a5/a4)
- {
- double f = A[5] / A[4];
- A[5] = 0.0;
- A[8] -= A[7] * f;
-
- B[2] -= B[1] * f;
- }
-
- // back substitute:
-
- // row 1 -= row 2 * (a7/a8)
- {
- double f = A[7] / A[8];
- A[7] = 0.0;
-
- B[1] -= B[2] * f;
- }
-
- // row 0 -= row 2 * (a6/a8)
- {
- double f = A[6] / A[8];
- A[6] = 0.0;
+ for (int i = 0; i < N; ++i) {
+ for (int j = i+1; j < N; ++j) {
+ // row j -= row i * (a[i,j] / a[i,i])
+ double f = A[j+i*N] / A[i+i*N];
+
+ A[j+i*N] = 0.0;
+ for (int k = i+1; k < N; ++k) {
+ A[j+k*N] -= A[i+k*N] * f;
+ }
- // row 0 -= row 1 * (a3/a4)
- {
- double f = A[3] / A[4];
- A[3] = 0.0;
-
- B[0] -= B[1] * f;
+ // back-substitute
+ for (int i = N; i --> 0; ) {
+ for (int j = i; j --> 0; ) {
+ // row j -= row i * (a[j,j] / a[j,i])
+ double f = A[i+j*N] / A[j+j*N];
+
+ // A[j+i*N] = 0.0;
+ B[j] -= B[i] * f;
+ }
- x[0] = B[0] / A[0];
- x[1] = B[1] / A[4];
- x[2] = B[2] / A[8];
+ for (int i = 0; i < N; ++i) {
+ x[i] = B[i] / A[i+i*N];
+ }
}
// Give an OK starting estimate for the least squares, by numerical integration
}
// Give an OK starting estimate for the least squares, by numerical integration
mat_mul_trans(matA, curve.size(), 3, dbeta, curve.size(), 1, matATdb);
// solve
mat_mul_trans(matA, curve.size(), 3, dbeta, curve.size(), 1, matATdb);
// solve
- solve3x3(matATA, dlambda, matATdb);
+ solve_matrix<3>(matATA, dlambda, matATdb);
A += dlambda[0];
mu += dlambda[1];
A += dlambda[0];
mu += dlambda[1];