Main Page | Class List | File List | Class Members | File Members

mat.c File Reference

#include <assert.h>
#include <math.h>
#include <stdlib.h>

Go to the source code of this file.

Defines

#define C(i, j)   c[i*cc+j]
#define A(i, j)   _a[i*ac+j]
#define B(i, j)   _b[i*bc+j]

Functions

void mat_zero (void *c, int cr, int cc)
void mat_one (void *_c, int cr, int cc)
void mat_copy (void *a, int ar, int ac, void *c, int cr, int cc)
void mat_add (void *a, int ar, int ac, void *b, int br, int bc, void *c, int cr, int cc)
void mat_sub (void *a, int ar, int ac, void *b, int br, int bc, void *c, int cr, int cc)
void mat_mul (void *a, int ar, int ac, void *b, int br, int bc, void *c, int cr, int cc)
void mat_mul_tn (void *a, int ar, int ac, void *b, int br, int bc, void *c, int cr, int cc)
void mat_mul_nt (void *a, int ar, int ac, void *b, int br, int bc, void *c, int cr, int cc)
int mat_similarity (void *a, int ar, int ac, void *b, int br, int bc, void *c, int cr, int cc)
int sym_factor (void *a, int ar, int ac, void *c, int cr, int cc)
void sym_rdiv (void *a, int ar, int ac, void *b, int br, int bc, void *c, int cr, int cc)
void sym_ldiv (void *a, int ar, int ac, void *b, int br, int bc, void *c, int cr, int cc)

Define Documentation

#define A (  i,
j   )     _a[i*ac+j]
 

Referenced by mat_mul(), mat_mul_nt(), mat_mul_tn(), sym_factor(), sym_ldiv(), and sym_rdiv().

#define B (  i,
j   )     _b[i*bc+j]
 

Referenced by mat_mul(), mat_mul_nt(), mat_mul_tn(), sym_ldiv(), and sym_rdiv().

#define C (  i,
j   )     c[i*cc+j]
 

Referenced by mat_mul(), mat_mul_nt(), mat_mul_tn(), mat_one(), sym_factor(), sym_ldiv(), and sym_rdiv().

Function Documentation

void mat_add (  void *  a,
int  ar,
int  ac,
void *  b,
int  br,
int  bc,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 67 of file mat.c.

Referenced by kalman_update().

00070 {
00071   double *_a = (double *)a;
00072   double *_b = (double *)b;
00073   double *_c = (double *)c;
00074   int i;
00075 
00076   assert(ar == br && br == cr && ac == bc && bc == cc);
00077 
00078   for (i = 0; i < ar*ac; i++)
00079     _c[i] = _a[i] + _b[i];
00080 }

void mat_copy (  void *  a,
int  ar,
int  ac,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 53 of file mat.c.

Referenced by kalman_update().

00055 {
00056   double *_a = (double *)a;
00057   double *_c = (double *)c;
00058   int i;
00059 
00060   assert(ar == cr && ac == cc);
00061 
00062   for (i = 0; i < ar*ac; i++)
00063     _c[i] = _a[i];
00064 }

void mat_mul (  void *  a,
int  ar,
int  ac,
void *  b,
int  br,
int  bc,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 100 of file mat.c.

References A, B, and C.

Referenced by kalman_update(), and mat_similarity().

00103 {
00104   double *_a = (double *)a;
00105   double *_b = (double *)b;
00106   double *_c = (double *)c;
00107   double s;
00108   int i, j, k;
00109 
00110 #define A(i,j) _a[i*ac+j]
00111 #define B(i,j) _b[i*bc+j]
00112 #define C(i,j) _c[i*cc+j]
00113 
00114   assert(ar == cr && ac == br && bc == cc);
00115 
00116   for (i = 0; i < cr; i++)
00117     for (j = 0; j < cc; j++)
00118       {
00119         s = 0.;
00120         for (k = 0; k < ac; k++)
00121           s += A(i,k)*B(k,j);
00122         C(i,j) = s;
00123       }
00124 #undef A
00125 #undef B
00126 #undef C
00127 }

void mat_mul_nt (  void *  a,
int  ar,
int  ac,
void *  b,
int  br,
int  bc,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 160 of file mat.c.

References A, B, and C.

Referenced by kalman_update(), and mat_similarity().

00163 {
00164   double *_a = (double *)a;
00165   double *_b = (double *)b;
00166   double *_c = (double *)c;
00167   double s;
00168   int i, j, k;
00169 
00170 #define A(i,j) _a[i*ac+j]
00171 #define B(i,j) _b[i*bc+j]
00172 #define C(i,j) _c[i*cc+j]
00173 
00174   assert(ar == cr && ac == bc && br == cc);
00175 
00176   for (i = 0; i < cr; i++)
00177     for (j = 0; j < cc; j++)
00178       {
00179         s = 0.;
00180         for (k = 0; k < ac; k++)
00181           s += A(i,k)*B(j,k);
00182         C(i,j) = s;
00183       }
00184 #undef A
00185 #undef B
00186 #undef C
00187 }

void mat_mul_tn (  void *  a,
int  ar,
int  ac,
void *  b,
int  br,
int  bc,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 130 of file mat.c.

References A, B, and C.

00133 {
00134   double *_a = (double *)a;
00135   double *_b = (double *)b;
00136   double *_c = (double *)c;
00137   double s;
00138   int i, j, k;
00139 
00140 #define A(i,j) _a[i*ac+j]
00141 #define B(i,j) _b[i*bc+j]
00142 #define C(i,j) _c[i*cc+j]
00143 
00144   assert(ac == cr && ar == br && bc == cc);
00145 
00146   for (i = 0; i < cr; i++)
00147     for (j = 0; j < cc; j++)
00148       {
00149         s = 0.;
00150         for (k = 0; k < ar; k++)
00151           s += A(k,i)*B(k,j);
00152         C(i,j) = s;
00153       }
00154 #undef A
00155 #undef B
00156 #undef C
00157 }

void mat_one (  void *  _c,
int  cr,
int  cc
) 
 

Definition at line 37 of file mat.c.

References C, and mat_zero().

Referenced by kalman_update().

00038 {
00039   double *c = (double *)_c;
00040   int i;
00041 
00042   assert(cr == cc);
00043 
00044   mat_zero(c, cr, cc);
00045 
00046 #define C(i,j) c[i*cc+j]
00047   for (i = 0; i < cr; i++)
00048     C(i,i) = 1.;
00049 #undef C
00050 }

int mat_similarity (  void *  a,
int  ar,
int  ac,
void *  b,
int  br,
int  bc,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 191 of file mat.c.

References mat_mul(), and mat_mul_nt().

Referenced by kalman_update().

00194 {
00195   void *t;
00196   assert(ac==br && br == bc && ar==cr && cr==cc);
00197 
00198   t = malloc(ar*bc*sizeof(double));
00199   if (!t) return 1;             /* failure */
00200   mat_mul(a,ar,ac, b,br,bc, t,ar,bc);
00201   mat_mul_nt(t,ar,bc, a,ar,ac, c,cr,cc);
00202   free(t);
00203   return 0;
00204 }

void mat_sub (  void *  a,
int  ar,
int  ac,
void *  b,
int  br,
int  bc,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 84 of file mat.c.

Referenced by kalman_update().

00087 {
00088   double *_a = (double *)a;
00089   double *_b = (double *)b;
00090   double *_c = (double *)c;
00091   int i;
00092 
00093   assert(ar == br && br == cr && ac == bc && bc == cc);
00094 
00095   for (i = 0; i < ar*ac; i++)
00096     _c[i] = _a[i] - _b[i];
00097 }

void mat_zero (  void *  c,
int  cr,
int  cc
) 
 

Definition at line 27 of file mat.c.

Referenced by mat_one().

00028 {
00029   double *_c = (double *)c;
00030   int i;
00031 
00032   for (i = 0; i < cr*cc; i++)
00033     _c[i] = 0.;
00034 }

int sym_factor (  void *  a,
int  ar,
int  ac,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 211 of file mat.c.

References A, and C.

Referenced by kalman_update().

00213 {
00214   double *_a = (double *)a;
00215   double *_c = (double *)c;
00216   double d, s;
00217   int i, j, k;
00218 
00219 #define A(i,j) _a[i*ac+j]
00220 #define C(i,j) _c[i*cc+j]
00221 
00222   assert(ar == ac && ac == cr && cr == cc); /* must be square */
00223 
00224   for (j = 0; j < cc; j++)      /* columns of c */
00225     {
00226       s = A(j,j);
00227       for (i = 0; i < j; i++)   /* rows of c */
00228         s -= C(j,i)*C(j,i);
00229       if (s < 0.) return 1;     /* failure (singular matrix) */
00230       d = C(j,j) = sqrt(s);
00231       for (i = j+1; i < cc; i++) /* columns of c */
00232         {
00233           s = A(i,j);
00234           for (k = 0; k < j; k++)
00235             s -= C(j,k)*C(i,k);
00236           C(i,j) = s/d;
00237         }
00238     }
00239   return 0;
00240 #undef A
00241 #undef C
00242 }

void sym_ldiv (  void *  a,
int  ar,
int  ac,
void *  b,
int  br,
int  bc,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 293 of file mat.c.

References A, B, and C.

00296 {
00297   double *_a = (double *)a;
00298   double *_b = (double *)b;
00299   double *_c = (double *)c;
00300   double s;
00301   int i, j, k;
00302 
00303 #define A(i,j) _a[i*ac+j]
00304 #define B(i,j) _b[i*bc+j]
00305 #define C(i,j) _c[i*cc+j]
00306 
00307   assert(ar == cr && ac == br && ar == ac && bc == cc);
00308 
00309   for (j = 0; j < cc; j++)      /* columns of c */
00310     for (i = 0; i < cr; i++)    /* rows of c */
00311       {
00312         s = B(i,j);
00313         for (k = 0; k < i; k++)
00314           s -= A(i,k)*C(k,j);
00315         s = C(i,j) = s/A(i,i);
00316       }
00317 
00318   for (j = 0; j < cc; j++)      /* columns of c */
00319     for (i = cr; i--; )         /* rows of c */
00320       {
00321         s = C(i,j);
00322         for (k = i+1; k < cr; k++)
00323           s -= A(k,i)*C(k,j);
00324         C(i,j) = s/A(i,i);
00325       }
00326       
00327 #undef A
00328 #undef B
00329 #undef C
00330 }

void sym_rdiv (  void *  a,
int  ar,
int  ac,
void *  b,
int  br,
int  bc,
void *  c,
int  cr,
int  cc
) 
 

Definition at line 248 of file mat.c.

References A, B, and C.

Referenced by kalman_update().

00251 {
00252   double *_a = (double *)a;
00253   double *_b = (double *)b;
00254   double *_c = (double *)c;
00255   double s;
00256   int i, j, k;
00257 
00258 #define A(i,j) _a[i*ac+j]
00259 #define B(i,j) _b[i*bc+j]
00260 #define C(i,j) _c[i*cc+j]
00261 
00262   assert(ar == cr && ac == cc && br == bc && ac == br);
00263 
00264   for (i = 0; i < ar; i++)      /* rows of a */
00265     {
00266       for (j = 0; j < ac; j++)  /* cols of a */
00267         {
00268           s = A(i,j);
00269           for (k = 0; k < j; k++)
00270             s -= C(i,k)*B(j,k);
00271           C(i,j) = s/B(j,j);
00272         }
00273     }
00274 
00275   for (i = 0; i < cr; i++)      /* rows of c */
00276     {
00277       for (j = cc; j--; )       /* cols of c */
00278         {
00279           s = C(i,j);
00280           for (k = j+1; k < cc; k++)
00281             s -= C(i,k)*B(k,j);
00282           C(i,j) = s/B(j,j);
00283         }
00284     }
00285 #undef A
00286 #undef B
00287 #undef C
00288 }

© sourcejam.com 2005-2008