1 #include <math.h>
2 #include <stdlib.h>
3 #include <stdio.h>
4 #include <string.h>
5 #include "sqrmat.h"
6
7
8 /*--------------------------------------------------------------*/
9
sm_iktk(sqrmat * K)10 sqrmat * sm_iktk( sqrmat *K )
11 {
12 int n , i,j,p ;
13 double *mat , *nat , sum ;
14 sqrmat *N ;
15
16 n = K->n ;
17 INIT_SQRMAT(N,n) ;
18 mat = K->mat ; nat = N->mat ;
19 for( j=0 ; j < n ; j++ ){
20 for( i=0 ; i <= j ; i++ ){
21 sum = -MAT(i,j) - MAT(j,i) ;
22 for( p=0 ; p < n ; p++ ) sum += MAT(p,i)*MAT(p,j) ;
23 NAT(i,j) = sum ;
24 }
25 for( i=0 ; i < j ; i++ ) NAT(j,i) = NAT(i,j) ;
26 NAT(j,j) += 1.0 ;
27 }
28 return N ;
29 }
30
31 /*--------------------------------------------------------------*/
32
sm_choleski(sqrmat * A)33 int sm_choleski( sqrmat *A )
34 {
35 int n , i,j,p ;
36 double *mat , sum ;
37
38 n = A->n ; mat = A->mat ;
39 if( MAT(0,0) <= 0.0 ) return 0 ;
40 MAT(0,0) = sqrt(MAT(0,0)) ;
41 for( j=1 ; j < n ; j++ ) MAT(0,j) = 0.0 ;
42 for( i=1 ; i < n ; i++ ){
43 MAT(i,0) /= MAT(0,0) ;
44 for( j=1 ; j <= i ; j++ ){
45 sum = MAT(i,j) ;
46 for( p=0 ; p < j ; p++ ) sum -= MAT(i,p)*MAT(j,p) ;
47 if( j < i ) MAT(i,j) = sum / MAT(j,j) ;
48 else if( sum > 0.0 ) MAT(i,i) = sqrt(sum) ;
49 else return 0 ;
50 }
51 for( ; j < n ; j++ ) MAT(i,j) = 0.0 ;
52 }
53 return 1 ;
54 }
55
56 /*--------------------------------------------------------------*/
57
sm_lndet_iktk(sqrmat * K)58 double sm_lndet_iktk( sqrmat *K )
59 {
60 sqrmat *N ;
61 double *mat , sum ; int i,n ;
62
63 N = sm_iktk( K ) ; i = sm_choleski( N ) ;
64 if( i == 0 ){ KILL_SQRMAT(N); return 0.0; }
65 mat = N->mat ; n = N->n ; sum = 0.0 ;
66 for( i=0 ; i < n ; i++ ) sum += log(MAT(i,i)) ;
67 sum += sum; /* need to double this for both upper and lower*/
68 KILL_SQRMAT(N) ; return sum ;
69 }
70
71 /*--------------------------------------------------------------*/
72
sm_copy(sqrmat * A)73 sqrmat * sm_copy( sqrmat *A )
74 {
75 sqrmat *B ; int n=A->n ;
76 INIT_SQRMAT(B,n) ;
77 memcpy((void *)B->mat,(void *)A->mat,sizeof(double)*n*n) ;
78 return B ;
79 }
80
81 /*--------------------------------------------------------------*/
82
sm_transpose(sqrmat * A)83 sqrmat * sm_transpose( sqrmat *A )
84 {
85 sqrmat *B ; int n=A->n , i,j ; double *mat,*nat ;
86 INIT_SQRMAT(B,n) ;
87 mat = A->mat ; nat = B->mat ;
88 for( i=0 ; i < n ; i++ ){
89 NAT(i,i) = MAT(i,i) ;
90 for( j=0 ; j < i ; j++ ){
91 NAT(i,j) = MAT(j,i) ; NAT(j,i) = MAT(i,j) ;
92 }
93 }
94 return B ;
95 }
96
97 /*--------------------------------------------------------------*/
98
sm_mult(sqrmat * A,sqrmat * B)99 sqrmat * sm_mult( sqrmat *A , sqrmat *B )
100 {
101 sqrmat *C ; int n=A->n , i,j,k ; double *mat,*nat,*pat , sum ;
102 INIT_SQRMAT(C,n) ;
103 mat = A->mat ; nat = B->mat ; pat = C->mat ;
104 for( i=0 ; i < n ; i++ ){
105 for( j=0 ; j < n ; j++ ){
106 sum = 0.0 ;
107 for( k=0 ; k < n ; k++ ) sum += MAT(i,k)*NAT(k,j) ;
108 PAT(i,j) = sum ;
109 }
110 }
111 return C ;
112 }
113
114
sm_subtract(sqrmat * A,sqrmat * B)115 sqrmat * sm_subtract( sqrmat *A , sqrmat *B )
116 {
117 sqrmat *C ; int n=A->n , i,j,k ; double *mat,*nat,*pat , sum ;
118 INIT_SQRMAT(C,n) ;
119 mat = A->mat ; nat = B->mat ; pat = C->mat ;
120 for( i=0 ; i < n ; i++ ){
121 for( j=0 ; j < n ; j++ ){
122 PAT(i,j) = MAT(i,j) - NAT(i,j) ;
123 }
124 }
125 return C ;
126 }
127
sm_add(sqrmat * A,sqrmat * B)128 sqrmat * sm_add( sqrmat *A , sqrmat *B )
129 {
130 sqrmat *C ; int n=A->n , i,j,k ; double *mat,*nat,*pat;
131 INIT_SQRMAT(C,n) ;
132 mat = A->mat ; nat = B->mat ; pat = C->mat ;
133 for( i=0 ; i < n ; i++ ){
134 for( j=0 ; j < n ; j++ ){
135 PAT(i,j) = MAT(i,j) + NAT(i,j) ;
136 }
137 }
138 return C ;
139 }
140
141 /* create identity matrix of size nxn */
sm_identity(int n)142 sqrmat * sm_identity(int n)
143 {
144 sqrmat *i_mat;
145 double *mat;
146 int i;
147
148 INIT_SQRMAT(i_mat, n);
149 mat = i_mat->mat;
150 for(i=0;i<n;i++) {
151 MAT(i,i) = 1.0;
152 }
153 return(i_mat);
154 }
155
156
157 /* compute the large dot product of two matrices */
158 /* equals the trace of the product */
sm_dot(sqrmat * A,sqrmat * B)159 double sm_dot(sqrmat *A, sqrmat *B)
160 {
161 int n=A->n , i,j;
162 double *mat,*nat;
163 double sum = 0.0;
164 mat = A->mat ; nat = B->mat ;
165 for( i=0 ; i < n ; i++ ){
166 for( j=0 ; j < n ; j++ ){
167 sum += MAT(i,j) * NAT(i,j) ;
168 }
169 }
170 return(sum) ;
171 }
172
173 /* compute the trace of the matrix */
174 /* trace = sum of diagonal elements */
sm_trace(sqrmat * A)175 double sm_trace(sqrmat *A)
176 {
177 int n=A->n , i;
178 double *mat;
179 double sum = 0.0;
180 mat = A->mat ;
181 for( i=0 ; i < n ; i++ ){
182 sum += MAT(i,i);
183 }
184 return(sum) ;
185 }
186
187 /* scale matrix by factor*/
188 /* if newmatrix flag is set, return a new matrix with result */
189 /* otherwise scale each element of array in place overwriting */
190 /* elements */
sm_scale(sqrmat * A,double sc_factor,int newmatrix)191 sqrmat * sm_scale(sqrmat *A, double sc_factor, int newmatrix)
192 {
193 int n=A->n, i, j;
194 double *mat, *nat;
195 sqrmat *B = NULL;
196
197 mat = A->mat;
198 if(newmatrix) {
199 INIT_SQRMAT(B,n);
200 nat = B->mat;
201 }
202 else
203 nat = mat;
204 for(i=0;i<n;i++)
205 for(j=0;j<n;j++)
206 NAT(i,j) = sc_factor * MAT(i,j);
207
208 return(B);
209 }
210
211
212
213 /*--------------------------------------------------------------*/
214 #if 0
215 int main( int argc , char *argv[] )
216 {
217 int n,nn , ii ;
218 sqrmat *KK , *AA , *AAtr, *CH ;
219 double *mat, *nat , val ;
220
221 if( argc < 2 ) exit(1) ;
222 n=nn = (int)strtod(argv[1],NULL); if( nn < 2 ) exit(1);
223
224 INIT_SQRMAT(KK,nn) ; mat = KK->mat ;
225 for( ii=1 ; ii < nn ; ii++ ){
226 MAT(ii,ii-1) = (double)ii ;
227 MAT(ii-1,ii) = (double)(ii*ii) ;
228 }
229 DUMP_SQRMAT("KK",KK) ;
230 val = sm_lndet_iktk(KK) ; printf("ln[det[]] = %g\n",val) ;
231
232 AA = sm_iktk( KK ) ;
233 DUMP_SQRMAT("[I-K'][I-K]",AA) ;
234
235 ii = sm_choleski( AA ) ;
236 if( ii < 1 ) exit(1) ;
237 DUMP_SQRMAT("Choleski",AA) ;
238
239 AAtr = sm_transpose(AA) ;
240 CH = sm_mult( AA , AAtr ) ;
241 DUMP_SQRMAT("[Ch][Ch']",CH) ;
242
243 exit(0) ;
244 }
245 #endif
246