// Utility function to evaluate a given Lagrange basis function at a point. // // Usage: l = lagrange_basis(x, i, z); // // inputs: x Mat vector of length n+1, containing the interpolation nodes // i integer indicating which Lagrange basis function to evaluate // z double location to evaluate basis function // outputs: p value of l(z) // double lagrange_basis(Mat &x, int i, double z) { double l = 1.0; // initialize basis function double *xd = x.get_data(); // access data array (for increased speed) for (int j=0; j<x.Rows()*x.Cols(); j++) if (j != i) l *= (z - xd[j]) / (xd[i] - xd[j]); return l; }
// performs forwards substitution on the linear system L*x = b, filling in the input Mat x int FwdSub(Mat &Lmat, Mat &xvec, Mat &bvec) { // check that matrix sizes match if (Lmat.Rows() != bvec.Rows() || Lmat.Rows() != Lmat.Cols() || bvec.Cols() != 1 || xvec.Rows() != Lmat.Rows() || xvec.Cols() != 1) { cerr << "FwdSub error, illegal matrix/vector dimensions\n"; cerr << " Mat is " << Lmat.Rows() << " x " << Lmat.Cols() << ", rhs is " << bvec.Rows() << " x " << bvec.Cols() << ", solution is " << xvec.Rows() << " x " << xvec.Cols() << endl; return 1; } // get the matrix size long int n = Lmat.Rows(); // access the data arrays double *L = Lmat.get_data(); double *x = xvec.get_data(); double *b = bvec.get_data(); // copy b into x xvec = bvec; // analyze matrix for typical nonzero magnitude double Lmax = Lmat.MaxNorm(); // perform column-oriented Forwards Substitution algorithm for (long int j=0; j<n; j++) { // check for nonzero matrix diagonal if (fabs(L[IDX(j,j,n)]) < STOL*Lmax) { cerr << "FwdSub error: singular matrix!\n"; return 1; } // solve for this row of solution x[j] /= L[IDX(j,j,n)]; // update all remaining rhs for (long int i=j+1; i<n; i++) x[i] -= L[IDX(i,j,n)]*x[j]; } // return success return 0; }
// performs backwards substitution on the linear system U*x = b, filling in the input Mat x int BackSub(Mat &Umat, Mat &xvec, Mat &bvec) { // check that matrix sizes match if (Umat.Rows() != bvec.Rows() || Umat.Rows() != Umat.Cols() || bvec.Cols() != 1 || xvec.Rows() != Umat.Rows() || xvec.Cols() != 1) { cerr << "BackSub error, illegal matrix/vector dimensions\n"; cerr << " Mat is " << Umat.Rows() << " x " << Umat.Cols() << ", rhs is " << bvec.Rows() << " x " << bvec.Cols() << ", solution is " << xvec.Rows() << " x " << xvec.Cols() << endl; return 1; } // get the matrix size long int n = Umat.Rows(); // access the data arrays double *U = Umat.get_data(); double *x = xvec.get_data(); double *b = bvec.get_data(); // copy b into x xvec = bvec; // analyze matrix for typical nonzero magnitude double Umax = Umat.MaxNorm(); // perform column-oriented Backwards Substitution algorithm for (long int j=n-1; j>=0; j--) { // check for nonzero matrix diagonal if (fabs(U[IDX(j,j,n)]) < STOL*Umax) { cerr << "BackSub error: numerically singular matrix!\n"; return 1; } // solve for this row of solution x[j] /= U[IDX(j,j,n)]; // update all remaining rhs for (long int i=0; i<j; i++) x[i] -= U[IDX(i,j,n)]*x[j]; } // return success return 0; }
// solves a linear system A*x = b, filling in the input Mat x int Solve(Mat &Amat, Mat &xvec, Mat &bvec) { // check that matrix sizes match if (Amat.Rows() != bvec.Rows() || Amat.Rows() != Amat.Cols() || bvec.Cols() != 1 || xvec.Rows() != Amat.Rows() || xvec.Cols() != 1) { cerr << "Solve error, illegal matrix/vector dimensions\n"; cerr << " Mat is " << Amat.Rows() << " x " << Amat.Cols() << ", rhs is " << bvec.Rows() << " x " << bvec.Cols() << ", solution is " << xvec.Rows() << " x " << xvec.Cols() << endl; return 1; } // create temporary variables long int i, j, k, p, n; double m, tmp, Amax; // access the data arrays double *A = Amat.get_data(); double *b = bvec.get_data(); // determine maximum absolute entry in A (for singularity check later) Amax = Amat.MaxNorm(); // perform Gaussian elimination to convert A,b to an upper-triangular system n = Amat.Rows(); for (k=0; k<n-1; k++) { // loop over diagonals // find the pivot row p p=k; for (i=k; i<n; i++) if (fabs(A[IDX(i,k,n)]) > fabs(A[IDX(p,k,n)])) p=i; // swap rows in A for (j=k; j<n; j++) { tmp = A[IDX(p,j,n)]; A[IDX(p,j,n)] = A[IDX(k,j,n)]; A[IDX(k,j,n)] = tmp; } // swap rows in b tmp = b[p]; b[p] = b[k]; b[k] = tmp; // check for nonzero matrix diagonal if (fabs(A[IDX(k,k,n)]) < STOL*Amax) { cerr << "Solve error: numerically singular matrix!\n"; return 1; } // perform elimination using row k for (i=k+1; i<n; i++) // store multipliers in column below pivot A[IDX(i,k,n)] /= A[IDX(k,k,n)]; for (j=k+1; j<n; j++) // loop over columns of A, to right of pivot for (i=k+1; i<n; i++) // update rows in column A[IDX(i,j,n)] -= A[IDX(i,k,n)]*A[IDX(k,j,n)]; for (i=k+1; i<n; i++) // update entries in b b[i] -= A[IDX(i,k,n)]*b[k]; } // check for singularity at end (only need to check final diagonal entry) if (fabs(A[IDX(n-1,n-1,n)]) < STOL*Amax) { cerr << "Solve error: numerically singular matrix!\n"; return 1; } // check for singularity at end (only need to check final diagonal entry) if (fabs(A[IDX(n-1,n-1,n)]) < STOL*Amax) { cerr << "Solve error: numerically singular matrix!\n"; return 1; } // perform Backwards Substitution on result if (BackSub(Amat, xvec, bvec) != 0) { cerr << "Solve: error in BackSub call\n"; return 1; } // return success return 0; }