1 double sqrt(double);
2
axpy(int n,double a,double x[],double y[])3 void axpy(int n, double a, double x[], double y[])
4 {
5 for (int i = 0; i < n; i++) {
6 y[i] += a * x[i];
7 }
8 }
9
dot(int n,double x[],double y[])10 double dot(int n, double x[], double y[])
11 {
12 double result = 0.0;
13 for (int i =0; i < n; i++) {
14 result += x[i] * y[i];
15 }
16 return result;
17 }
18
scal(int n,double a,double x[])19 void scal(int n, double a, double x[])
20 {
21 for (int i = 0; i < n; i++) {
22 x[i] *= a;
23 }
24 }
25
26 /*
27 * Compute a * b, where a is upper triangular.
28 * The result is written into b.
29 */
triangular_multiply(int n,double a[],double b[])30 void triangular_multiply(int n, double a[], double b[]) {
31 for (int j = 0; j < n; j++) {
32 axpy(j, b[j], &a[j * n], b);
33 b[j] *= a[j + j * n];
34 }
35 }
36
37 /*
38 * Compute transpose(a) * b, where a is upper triangular.
39 * The result is written into b.
40 */
triangular_multiply_transpose(int n,double a[],double b[])41 void triangular_multiply_transpose(int n, double a[], double b[]) {
42 for (int j = n - 1; j >= 0; j--) {
43 b[j] *= a[j + j * n];
44 b[j] += dot(j, b, &a[j * n]);
45 }
46 }
47
48 /*
49 * Solve a * x = b, where a is upper triangular.
50 * The solution is written into b.
51 */
triangular_solve(int n,double a[],double b[])52 void triangular_solve(int n, double a[], double b[]) {
53 for (int k = n - 1; k >= 0; k--) {
54 b[k] /= a[k + k * n];
55 axpy(k, -b[k], &a[k * n], b);
56 }
57 }
58
59 /*
60 * Solve transpose(a) * x = b, where a is upper triangular.
61 * The solution is written into b.
62 */
triangular_solve_transpose(int n,double a[],double b[])63 void triangular_solve_transpose(int n, double a[], double b[]) {
64 for (int k = 0; k < n; k++) {
65 b[k] -= dot(k, &a[k * n], b);
66 b[k] /= a[k + k * n];
67 }
68 }
69
70 /*
71 * Invert a, where a is upper triangular.
72 * The inverse is written into a.
73 */
triangular_invert(int n,double a[])74 void triangular_invert(int n, double a[]) {
75 for (int k = 0; k < n; k++) {
76 a[k + k * n] = 1.0 / a[k + k * n];
77 scal(k, -a[k + k * n], &a[k * n]);
78
79 for (int j = k + 1; j < n; j++) {
80 axpy(k, a[k + j * n], &a[k * n], &a[j * n]);
81 a[k + j * n] *= a[k + k * n];
82 }
83 }
84 }
85
86 /*
87 * Find the upper triangular matrix r such that a = transpose(r) * r, where a is positive definite.
88 * The result is written into the upper triangle of a.
89 * Returns: 0 if successful;
90 * j>0 if the leading jth minor of a is not positive definite.
91 */
cholesky(int n,double a[])92 int cholesky(int n, double a[]) {
93 for (int j = 0; j < n; j++) {
94 for (int k = 0; k < j; k++) {
95 a[k + j * n] =
96 (a[k + j * n] - dot(k, &a[k * n], &a[j * n])) / a[k + k * n];
97 }
98
99 double s = a[j + j * n] - dot(j, &a[j * n], &a[j * n]);
100
101 if (s <= 0.0) {
102 return j + 1;
103 }
104
105 a[j + j * n] = sqrt(s);
106 }
107
108 return 0;
109 }