5dfb4363d62b42740e83ebf92df8f076c26a6749
1 #include <math.h>
2 #include <stdlib.h>
3 #include <valarray>
4 #include <cassert>
5 #include "conjugate_gradient.h"
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 */
17 /* lifted wholely from wikipedia. Well, apart from the bug in the wikipedia version. */
19 using std::valarray;
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 }
38 static double Linfty(valarray<double> const &vec) {
39 return std::max(vec.max(), -vec.min());
40 }
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 }
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