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 static double Linfty(valarray<double> const &vec) {
  39     return std::max(vec.max(), -vec.min());
  40 }
  41
  42 double
  43 inner(valarray<double> const &x,
  44       valarray<double> const &y) {
  45     double total = 0;
  46     for(unsigned i = 0; i < x.size(); i++)
  47         total += x[i]*y[i];
  48     return total;// (x*y).sum(); <- this is more concise, but ineff
  49 }
  50
  51 void
  52 conjugate_gradient(double **A,
  53                    double *x,
  54                    double *b,
  55                    unsigned n,
  56                    double tol,
  57                    unsigned max_iterations) {
  58     valarray<double> vA(n*n);
  59     valarray<double> vx(n);
  60     valarray<double> vb(n);
  61     for(unsigned i=0;i<n;i++) {
  62         vx[i]=x[i];
  63         vb[i]=b[i];
  64         for(unsigned j=0;j<n;j++) {
  65             vA[i*n+j]=A[i][j];
  66         }
  67     }
  68     conjugate_gradient(vA,vx,vb,n,tol,max_iterations);
  69     for(unsigned i=0;i<n;i++) {
  70         x[i]=vx[i];
  71     }
  72 }
  73 void
  74 conjugate_gradient(valarray<double> const &A,
  75                    valarray<double> &x,
  76                    valarray<double> const &b,
  77                    unsigned n, double tol,
  78                    unsigned max_iterations) {
  79     valarray<double> Ap(n), p(n), r(n);
  80     matrix_times_vector(A,x,Ap);
  81     r=b-Ap;
  82     double r_r = inner(r,r);
  83     unsigned k = 0;
  84     tol *= tol;
  85     while(k < max_iterations && r_r > tol) {
  86         k++;
  87         double r_r_new = r_r;
  88         if(k == 1)
  89             p = r;
  90         else {
  91             r_r_new = inner(r,r);
  92             p = r + (r_r_new/r_r)*p;
  93         }
  94         matrix_times_vector(A, p, Ap);
  95         double alpha_k = r_r_new / inner(p, Ap);
  96         x += alpha_k*p;
  97         r -= alpha_k*Ap;
  98         r_r = r_r_new;
  99     }
 100     printf("njh: %d iters, Linfty = %g L2 = %g\n", k,
 101     std::max(-r.min(), r.max()), sqrt(r_r));
 102     // x is solution
 103 }
 104 /*
 105   Local Variables:
 106   mode:c++
 107   c-file-style:"stroustrup"
 108   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 109   indent-tabs-mode:nil
 110   fill-column:99
 111   End:
 112 */
 113 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=4:softtabstop=4