← Back to branch summary

~hrg/hrg-packaging/zm

« back to all changes in this revision

Viewing changes to src/zm_le_minv.c

  • Committer: Yosuke Matsusaka
  • Date: 2016-01-12 04:52:39 UTC
  • Revision ID: yosuke.matsusaka@gmail.com-20160112045239-cy93cfjhdtpjf17w
initial import

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/zm_le_minv.c
 
1
/* ZM - Z's Mathematics Toolbox
 
2
 * Copyright (C) 1998 Tomomichi Sugihara (Zhidao)
 
3
 *
 
4
 * zm_le_minv - linear equation: determinant and inverse matrix.
 
5
 */
 
6
 
 
7
#include <zm/zm_le.h>
 
8
 
 
9
static zMat _zMulInvMat(zMat m1, zMat m2, zMat m, zIndex idx, zVec s);
 
10
static void _zBalancingMatDST(zMat m1, zMat m2, zVec s);
 
11
 
 
12
/* zMatDetDST
 
13
 * - determinant of matrix (destructive).
 
14
 */
 
15
double zMatDetDST(zMat m, zIndex idx)
 
16
{
 
17
  register int i, j, k, p, q;
 
18
  double det = 1.0;
 
19
 
 
20
  zIndexOrder( idx, 0 );
 
21
  for( i=0; i<zMatRowSizeNC(m); i++ ){
 
22
    p = zIndexElem( idx, i );
 
23
    if( p != zPivoting( m, idx, i, i ) ){
 
24
      det = -det;
 
25
      p = zIndexElem( idx, i );
 
26
    }
 
27
    if( zIsTiny( ( det *= zMatElem( m, p, i ) ) ) ) return 0;
 
28
    for( j=i+1; j<zMatRowSizeNC(m); j++ ){
 
29
      q = zIndexElem( idx, j );
 
30
      for( k=i+1; k<zMatRowSizeNC(m); k++ )
 
31
        zMatElem(m,q,k) -=
 
32
          zMatElem(m,p,k) / zMatElem(m,p,i) * zMatElem(m,q,i);
 
33
    }
 
34
  }
 
35
  return det;
 
36
}
 
37
 
 
38
/* zMatDet
 
39
 * - determinant of matrix.
 
40
 */
 
41
double zMatDet(zMat m)
 
42
{
 
43
  int n;
 
44
  double det = 0;
 
45
  zMat mcp;
 
46
  zIndex idx;
 
47
 
 
48
  if( !zMatIsSqr( m ) ){
 
49
    ZRUNERROR( ZM_ERR_NONSQR_MAT );
 
50
    return 0;
 
51
  }
 
52
  n = zMatRowSize( m );
 
53
  mcp = zMatClone( m );
 
54
  idx = zIndexCreate( n );
 
55
  if( mcp && idx )
 
56
    det = zMatDetDST( mcp, idx );
 
57
 
 
58
  zMatFree( mcp );
 
59
  zIndexFree( idx );
 
60
  return det;
 
61
}
 
62
 
 
63
/* zMatAdj
 
64
 * - adjoint matrix.
 
65
 */
 
66
zMat zMatAdj(zMat m, zMat adj)
 
67
{
 
68
  register int i, j, k, l, u, v;
 
69
  zMat smat;
 
70
  zIndex idx;
 
71
 
 
72
  if( !zMatIsSqr(m) || !zMatIsSqr(adj) ){
 
73
    ZRUNERROR( ZM_ERR_NONSQR_MAT );
 
74
    return NULL;
 
75
  }
 
76
  if( !zMatSizeIsEqual(m,adj) ){
 
77
    ZRUNERROR( ZM_ERR_SIZMIS_MAT );
 
78
    return NULL;
 
79
  }
 
80
  smat = zMatAllocSqr( zMatRowSizeNC(m)-1 );
 
81
  idx = zIndexCreate( zMatRowSizeNC(smat) );
 
82
  if( !smat || !idx ){
 
83
    ZALLOCERROR();
 
84
    adj = NULL;
 
85
    goto TERMINATE;
 
86
  }
 
87
  for( i=0; i<zMatRowSizeNC(m); i++ ){
 
88
    for( j=0; j<zMatColSizeNC(m); j++ ){
 
89
      for( k=u=0; k<zMatRowSizeNC(m); k++ ){
 
90
        if( k == i ) continue;
 
91
        for( l=v=0; l<zMatColSizeNC(m); l++ ){
 
92
          if( l == j ) continue;
 
93
          zMatSetElem( smat, u, v, zMatElem(m,k,l) );
 
94
          v++;
 
95
        }
 
96
        u++;
 
97
      }
 
98
      zMatSetElem( adj, j, i,
 
99
        (i+j)%2 ? -zMatDetDST( smat, idx ) : zMatDetDST( smat, idx ) );
 
100
    }
 
101
  }
 
102
 TERMINATE:
 
103
  zMatFree( smat );
 
104
  zIndexFree( idx );
 
105
  return adj;
 
106
}
 
107
 
 
108
/* (static)
 
109
 * _zBalancingMatDST
 
110
 * - directly make a matrix row-balanced and column-balanced.
 
111
 */
 
112
void _zBalancingMatDST(zMat m1, zMat m2, zVec s)
 
113
{
 
114
  register int i, j;
 
115
  double *mp1, *mp2, tmp;
 
116
 
 
117
  /* column balancing */
 
118
  if( s )
 
119
    for( i=0; i<zMatColSizeNC(m1); i++ ){
 
120
      zVecSetElem( s, i, fabs( zMatElem(m1,0,i) ) );
 
121
      for( j=1; j<zMatRowSizeNC(m1); j++ ){
 
122
        tmp = fabs( zMatElem(m1,j,i) );
 
123
        if( tmp > zVecElem(s,i) ) zVecSetElem( s, i, tmp );
 
124
      }
 
125
      if( zVecElem(s,i) == 0 ) continue;
 
126
      /* inverse column-balancing factor */
 
127
      zVecSetElem( s, i, 1.0 / zVecElem(s,i) );
 
128
      for( j=0; j<zMatRowSizeNC(m1); j++ )
 
129
        zMatElem(m1,j,i) *= zVecElem(s,i);
 
130
    }
 
131
  /* row balancing */
 
132
  for( mp1=zMatBuf(m1), mp2=zMatBuf(m2), i=0; i<zMatRowSizeNC(m1); mp1+=zMatColSizeNC(m1), mp2+=zMatColSizeNC(m2), i++ ){
 
133
    if( ( tmp = zDataAbsMax( mp1, zMatColSizeNC(m1), NULL ) ) == 0 )
 
134
      continue;
 
135
    zRawVecDivDRC( mp1, tmp, zMatColSizeNC(m1) );
 
136
    zRawVecDivDRC( mp2, tmp, zMatColSizeNC(m2) );
 
137
  }
 
138
}
 
139
 
 
140
/* (static)
 
141
 * _zMulInvMat
 
142
 * - inner operation of zMulInvMatMat and zMulMatInvMat.
 
143
 */
 
144
zMat _zMulInvMat(zMat m1, zMat m2, zMat m, zIndex idx, zVec s)
 
145
{
 
146
  register int i, j, k;
 
147
  int n, p, q;
 
148
  double head;
 
149
  double x;
 
150
 
 
151
  n = zMatRowSizeNC( m );
 
152
  _zBalancingMatDST( m1, m2, s );
 
153
  /* forward elimination */
 
154
  for( i=0; i<n; i++ ){
 
155
    p = zPivoting( m1, idx, i, i );
 
156
    if( ( head = zMatElem(m1,p,i) ) == 0 ){
 
157
      ZRUNERROR( ZM_ERR_LE_SINGULAR );
 
158
      return NULL;
 
159
    }
 
160
    head = 1.0 / head;
 
161
    zMatSetElem( m1, p, i, 1 );
 
162
    for( j=i+1; j<n; j++ )
 
163
      zMatElem(m1,p,j) *= head;
 
164
    for( j=0; j<zMatColSizeNC(m2); j++ )
 
165
      zMatElem(m2,p,j) *= head;
 
166
    for( j=i+1; j<n; j++ ){
 
167
      q = zIndexElem( idx, j );
 
168
      if( !zIsTiny( head = zMatElem(m1,q,i) ) ){
 
169
        for( k=i+1; k<n; k++ )
 
170
          zMatElem(m1,q,k) -= zMatElem(m1,p,k) * head;
 
171
        for( k=0; k<zMatColSizeNC(m2); k++ )
 
172
          zMatElem(m2,q,k) -= zMatElem(m2,p,k) * head;
 
173
      }
 
174
      zMatSetElem( m1, q, i, 0 );
 
175
    }
 
176
  }
 
177
  /* backward elimination */
 
178
  for( i=n-1; i>=0; i-- ){
 
179
    p = zIndexElem( idx, i );
 
180
    for( j=0; j<zMatColSizeNC(m2); j++ ){
 
181
      x = zMatElem( m2, p, j );
 
182
      for( k=n-1; k>i; k-- )
 
183
        x -= zMatElem(m1,p,k)*zMatElem(m,k,j);
 
184
      zMatSetElem( m, i, j, x );
 
185
    }
 
186
  }
 
187
  if( s )
 
188
    for( i=0; i<n; i++ )
 
189
      zRawVecMulDRC( &zMatElem(m,i,0), zVecElem(s,i), zMatColSizeNC(m) );
 
190
  return m;
 
191
}
 
192
 
 
193
/* zMulInvMatMat
 
194
 * - multiplication of inverse matrix and matrix.
 
195
 */
 
196
zMat zMulInvMatMat(zMat m1, zMat m2, zMat m)
 
197
/* m = m1^-1 m2 */
 
198
{
 
199
  zMat mcp1, mcp2;
 
200
  zVec s;
 
201
  zIndex idx;
 
202
 
 
203
  if( !zMatIsSqr(m1) ){
 
204
    ZRUNERROR( ZM_ERR_NONSQR_MAT );
 
205
    return NULL;
 
206
  }
 
207
  if( zMatColSize(m1)!=zMatRowSize(m2) || !zMatSizeIsEqual(m2,m) ){
 
208
    ZRUNERROR( ZM_ERR_SIZMIS_MAT );
 
209
    return NULL;
 
210
  }
 
211
  mcp1 = zMatClone( m1 );
 
212
  mcp2 = zMatClone( m2 );
 
213
  idx = zIndexCreate( zMatRowSizeNC(m1) );
 
214
  s = zVecAlloc( zMatRowSizeNC(m1) );
 
215
  if( !mcp1 || !mcp2 || !idx || !s ||
 
216
      !_zMulInvMat( mcp1, mcp2, m, idx, s ) ) m = NULL;
 
217
 
 
218
  zMatFree( mcp1 );
 
219
  zMatFree( mcp2 );
 
220
  zIndexFree( idx );
 
221
  zVecFree( s );
 
222
  return m;
 
223
}
 
224
 
 
225
/* zMulMatInvMat
 
226
 * - multiplication of matrix and inverse matrix.
 
227
 */
 
228
zMat zMulMatInvMat(zMat m1, zMat m2, zMat m)
 
229
/* m = m1 m2^-1 */
 
230
{
 
231
  zMat mcp1, mcp2, mcp;
 
232
  zVec s;
 
233
  zIndex idx;
 
234
 
 
235
  if( !zMatIsSqr(m2) ){
 
236
    ZRUNERROR( ZM_ERR_NONSQR_MAT );
 
237
    return NULL;
 
238
  }
 
239
  if( zMatRowSize(m1)!=zMatColSize(m2) || !zMatSizeIsEqual(m1,m) ){
 
240
    ZRUNERROR( ZM_ERR_SIZMIS_MAT );
 
241
    return NULL;
 
242
  }
 
243
  mcp1 = zMatAlloc( zMatColSizeNC(m1), zMatRowSizeNC(m1) );
 
244
  mcp2 = zMatAlloc( zMatColSizeNC(m2), zMatRowSizeNC(m2) );
 
245
  mcp  = zMatAlloc( zMatColSizeNC(m), zMatRowSizeNC(m) );
 
246
  idx = zIndexCreate( zMatRowSizeNC(m2) );
 
247
  s = zVecAlloc( zMatRowSizeNC(m2) );
 
248
  if( !mcp1 || !mcp2 || !mcp || !idx || !s ) goto TERMINATE;
 
249
  zMatTNC( m1, mcp1 );
 
250
  zMatTNC( m2, mcp2 );
 
251
  if( _zMulInvMat( mcp2, mcp1, mcp, idx, s ) )
 
252
    zMatTNC( mcp, m );
 
253
  else
 
254
    m = NULL;
 
255
 
 
256
 TERMINATE:
 
257
  zMatFree( mcp1 );
 
258
  zMatFree( mcp2 );
 
259
  zMatFree( mcp );
 
260
  zIndexFree( idx );
 
261
  zVecFree( s );
 
262
  return m;
 
263
}
 
264
 
 
265
/* zMatInv
 
266
 * - inverse matrix.
 
267
 */
 
268
zMat zMatInv(zMat m, zMat im)
 
269
{
 
270
  zMatIdentNC( im );
 
271
  return zMulInvMatMat( m, im, im );
 
272
}
 
273
 
 
274
/* zMatInvHotelling
 
275
 * - inverse matrix by Hotelling's method.
 
276
 */
 
277
zMat zMatInvHotelling(zMat m, zMat im, double tol, int iter)
 
278
{
 
279
  register int i, j;
 
280
  zMat im2, tmp, mc, mn;
 
281
 
 
282
  if( !zMatIsSqr( m ) ){
 
283
    ZRUNERROR( ZM_ERR_NONSQR_MAT );
 
284
    return NULL;
 
285
  }
 
286
  if( !zMatSizeIsEqual( m, im ) ){
 
287
    ZRUNERROR( ZM_ERR_SIZMIS_MAT );
 
288
    return NULL;
 
289
  }
 
290
  im2 = zMatAllocSqr( zMatRowSizeNC(m) );
 
291
  tmp = zMatAllocSqr( zMatRowSizeNC(m) );
 
292
  if( !im2 || !tmp ){
 
293
    ZALLOCERROR();
 
294
    im = NULL;
 
295
    goto TERMINATE;
 
296
  }
 
297
 
 
298
  zMatT( m, im );
 
299
  zMatDivDRC( im, zMatSqrNorm(m) );
 
300
  ZITERINIT( iter );
 
301
  for( mc=im, mn=im2, i=0; i<iter; i++ ){
 
302
    /* update */
 
303
    zMulMatMatNC( m, mc, mn );
 
304
    zMulMatMatNC( mc, mn, tmp );
 
305
    zMatMul( mc, 2, mn );
 
306
    zMatSubNCDRC( mn, tmp );
 
307
    /* evaluate */
 
308
    zMulMatMatNC( m, mn, tmp );
 
309
    for( j=0; j<zMatRowSizeNC(tmp); j++ )
 
310
      zMatElem(tmp,j,j) -= 1.0;
 
311
    if( zMatIsTol( tmp, tol ) ){
 
312
      if( mc != im ) zMatCopy( mc, im );
 
313
      goto TERMINATE;
 
314
    }
 
315
    /* swap working memory */
 
316
    zSwap( zMat, mc, mn );
 
317
  }
 
318
  ZITERWARN( iter );
 
319
 
 
320
 TERMINATE:
 
321
  zMatFree( im2 );
 
322
  zMatFree( tmp );
 
323
  return im;
 
324
}