1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|
/*
* Matrix operations library
*
* Copyright (C) 1999-2000
* Thomas Sailer, <sailer@ife.ee.ethz.ch>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
/* AIX requires this to be the first thing in the file. */
#ifndef __GNUC__
# if HAVE_ALLOCA_H
# include <alloca.h>
# else
# ifdef _AIX
#pragma alloca
# else
# ifndef alloca /* predefined by HP cc +Olibcalls */
char *alloca ();
# endif
# endif
# endif
#endif
#include "mat.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
/*
* A el R^{d x d}
* This routine calculates G*G^T = A, where G is lower triangular, and then uses this to solve
* A*c=b for c
* G*G^T*c=b
* G*t=b
* G^T*c=t
*/
int frcholfactor(const float *a, float *g, unsigned int d)
{
unsigned int i, j, k;
float s, s1;
memset(g, 0, d*d*sizeof(g[0]));
for (i = 0; i < d; i++) {
s = a[i*d+i];
for (j = 0; j < i; j++)
s -= g[i*d+j] * g[i*d+j];
if (s <= 0) {
fprintf(stderr, "frcholfactor: matrix not positive definite a[%u][%u]=%g s=%g\n", i, i, a[i*d+i], s);
return -1;
}
s = 1/sqrt(s);
g[i*d+i] = s;
for (j = i+1; j < d; j++) {
s1 = 0;
for (k = 0; k < i; k++)
s1 += g[j*d+k] * g[i*d+k];
g[j*d+i] = s * (a[j*d+i] - s1);
}
}
return 0;
}
void frcholapply(const float *g, const float *b, float *c, unsigned int d)
{
float *t;
unsigned int i, j;
float s1;
t = alloca(d*sizeof(t[0]));
for (i = 0; i < d; i++) {
s1 = b[i];
for (j = 0; j < i; j++)
s1 -= g[i*d+j] * t[j];
t[i] = s1 * g[i*d+i];
}
for (i = d; i > 0; i--) {
s1 = t[i-1];
for (j = i; j < d; j++)
s1 -= g[j*d+(i-1)] * c[j];
c[i-1] = s1 * g[(i-1)*d+(i-1)];
}
}
int frchol(const float *a, const float *b, float *c, unsigned int d)
{
float *g;
g = alloca(d*d*sizeof(g[0]));
if (frcholfactor(a, g, d)) {
memset(c, 0, d*sizeof(c[0]));
return -1;
}
frcholapply(g, b, c, d);
return 0;
}
|
