src/libcola/conjugate_gradient.cpp

   1 #include <math.h>
   2 #include <stdlib.h>
   3 #include <valarray>
   4 #include <cassert>
   5 #include "conjugate_gradient.h"
   6
   7 /*
   8 * Authors:
   9 *   Nathan Hurst <njh@njhurst.com>
  10 *   Tim Dwyer <tgdwyer@gmail.com>
  11 *
  12 * Copyright (C) 2006 Authors
  13 *
  14 * Released under GNU LGPL.
  15 */
  16
  17 /* lifted wholely from wikipedia.  Well, apart from the bug in the wikipedia version. */
  18
  19 using std::valarray;
  20
  21 static void
  22 matrix_times_vector(valarray<double> const &matrix, /* m * n */
  23                     valarray<double> const &vec,  /* n */
  24                     valarray<double> &result) /* m */
  25 {
  26     unsigned n = vec.size();
  27     unsigned m = result.size();
  28     assert(m*n == matrix.size());
  29     const double* mp = &matrix[0];
  30     for (unsigned i = 0; i < m; i++) {
  31         double res = 0;
  32         for (unsigned j = 0; j < n; j++)
  33             res += *mp++ * vec[j];
  34         result[i] = res;
  35     }
  36 }
  37
  38 /*
  39 static double Linfty(valarray<double> const &vec) {
  40     return std::max(vec.max(), -vec.min());
  41 }
  42 */
  43
  44 double
  45 inner(valarray<double> const &x,
  46       valarray<double> const &y) {
  47     double total = 0;
  48     for(unsigned i = 0; i < x.size(); i++)
  49         total += x[i]*y[i];
  50     return total;// (x*y).sum(); <- this is more concise, but ineff
  51 }
  52
  53 void
  54 conjugate_gradient(double **A,
  55                    double *x,
  56                    double *b,
  57                    unsigned n,
  58                    double tol,
  59                    unsigned max_iterations) {
  60     valarray<double> vA(n*n);
  61     valarray<double> vx(n);
  62     valarray<double> vb(n);
  63     for(unsigned i=0;i<n;i++) {
  64         vx[i]=x[i];
  65         vb[i]=b[i];
  66         for(unsigned j=0;j<n;j++) {
  67             vA[i*n+j]=A[i][j];
  68         }
  69     }
  70     conjugate_gradient(vA,vx,vb,n,tol,max_iterations);
  71     for(unsigned i=0;i<n;i++) {
  72         x[i]=vx[i];
  73     }
  74 }
  75 void
  76 conjugate_gradient(valarray<double> const &A,
  77                    valarray<double> &x,
  78                    valarray<double> const &b,
  79                    unsigned n, double tol,
  80                    unsigned max_iterations) {
  81     valarray<double> Ap(n), p(n), r(n);
  82     matrix_times_vector(A,x,Ap);
  83     r=b-Ap;
  84     double r_r = inner(r,r);
  85     unsigned k = 0;
  86     tol *= tol;
  87     while(k < max_iterations && r_r > tol) {
  88         k++;
  89         double r_r_new = r_r;
  90         if(k == 1)
  91             p = r;
  92         else {
  93             r_r_new = inner(r,r);
  94             p = r + (r_r_new/r_r)*p;
  95         }
  96         matrix_times_vector(A, p, Ap);
  97         double alpha_k = r_r_new / inner(p, Ap);
  98         x += alpha_k*p;
  99         r -= alpha_k*Ap;
 100         r_r = r_r_new;
 101     }
 102     //printf("njh: %d iters, Linfty = %g L2 = %g\n", k,
 103     //std::max(-r.min(), r.max()), sqrt(r_r));
 104     // x is solution
 105 }
 106 /*
 107   Local Variables:
 108   mode:c++
 109   c-file-style:"stroustrup"
 110   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 111   indent-tabs-mode:nil
 112   fill-column:99
 113   End:
 114 */
 115 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=4:softtabstop=4