src/libcola/conjugate_gradient.cpp

	conjugate_gradient.cpp revision c787248292420aeb9b05e2e4852df8291904cdee
#include <math.h>
#include <stdlib.h>
#include <valarray>
#include <cassert>
#include "conjugate_gradient.h"

/*
* Authors:
*   Nathan Hurst <njh@njhurst.com>
*   Tim Dwyer <tgdwyer@gmail.com>
*
* Copyright (C) 2006 Authors
*
* Released under GNU LGPL.
*/

/* lifted wholely from wikipedia.  Well, apart from the bug in the wikipedia version. */

using std::valarray;

static void
matrix_times_vector(valarray<double> const &matrix, /* m * n */
            valarray<double> const &vec,  /* n */
            valarray<double> &result) /* m */
{
    unsigned n = vec.size();
    unsigned m = result.size();
    assert(m*n == matrix.size());
    const double* mp = &matrix[0];
    for (unsigned i = 0; i < m; i++) {
        double res = 0;
        for (unsigned j = 0; j < n; j++)
            res += *mp++ * vec[j];
        result[i] = res;
    }
}

static double Linfty(valarray<double> const &vec) {
    return std::max(vec.max(), -vec.min());
}

double
inner(valarray<double> const &x,
      valarray<double> const &y) {
    double total = 0;
    for(unsigned i = 0; i < x.size(); i++)
        total += x[i]*y[i];
    return total;// (x*y).sum(); <- this is more concise, but ineff
}

void
conjugate_gradient(double **A,
                   double *x,
                   double *b,
                   unsigned n,
                   double tol,
                   unsigned max_iterations) {
    valarray<double> vA(n*n);
    valarray<double> vx(n);
    valarray<double> vb(n);
    for(unsigned i=0;i<n;i++) {
        vx[i]=x[i];
        vb[i]=b[i];
        for(unsigned j=0;j<n;j++) {
            vA[i*n+j]=A[i][j];
        }
    }
    conjugate_gradient(vA,vx,vb,n,tol,max_iterations);
    for(unsigned i=0;i<n;i++) {
        x[i]=vx[i];
    }
}
void
conjugate_gradient(valarray<double> const &A,
           valarray<double> &x,
           valarray<double> const &b,
           unsigned n, double tol,
           unsigned max_iterations) {
    valarray<double> Ap(n), p(n), r(n);
    matrix_times_vector(A,x,Ap);
    r=b-Ap;
    double r_r = inner(r,r);
    unsigned k = 0;
    tol *= tol;
    while(k < max_iterations && r_r > tol) {
        k++;
        double r_r_new = r_r;
        if(k == 1)
            p = r;
        else {
            r_r_new = inner(r,r);
            p = r + (r_r_new/r_r)*p;
        }
        matrix_times_vector(A, p, Ap);
        double alpha_k = r_r_new / inner(p, Ap);
        x += alpha_k*p;
        r -= alpha_k*Ap;
        r_r = r_r_new;
    }
    //printf("njh: %d iters, Linfty = %g L2 = %g\n", k,
    //std::max(-r.min(), r.max()), sqrt(r_r));
    // x is solution
}
/*
  Local Variables:
  mode:c++
  c-file-style:"stroustrup"
  c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
  indent-tabs-mode:nil
  fill-column:99
  End:
*/
// vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=4:softtabstop=4