fish/asimplex/matrix.c

   1 /*****************************************************************************
   2  * Modul                : matrix.c                                           *
   3  * Zweck                : Funktionen für Matrixinversion, Matrizenmultipli-  *
   4  *                        kation und Matrizenzugriff                         *
   5  * Autor                : Stefan Förster                                     *
   6  *                                                                           *
   7  * Datum      | Version | Bemerkung                                          *
   8  * -----------|---------|--------------------------------------------------- *
   9  * 21.12.1988 |         | M()                                                *
  10  *            |         | SetM()                                             *
  11  *            |         | Mult()                                             *
  12  * 05.01.1989 |         | Invers()                                           *
  13  * 18.01.1989 |         | Mult() ohne M() und SetM()                         *
  14  * 30.01.1989 |         | Invers() ohne Verwendung von LRSubst(),LRPart())   *
  15  *            |         | Bug in M(): Typumwandlung (LONG) wegen Overflow    *
  16  *            |         | Bug in SetM(): Typumwandlung (LONG) wegen Overflow *
  17  * 06.02.1989 | 0.0     | Invers() ohne M() u. SetM(); Anpassung an ASimplex *
  18  *            |         | Mult() angepaßt an ASimplex                        *
  19  *            |         | M() angepaßt an ASimplex                           *
  20  *            |         | SetM() angepaßt an ASimplex                        *
  21  * 02.03.1989 | 0.1     | Bug in M(): if(flag==PHASE_I) statt if(PHASE_I)    *
  22  * 07.03.1989 | 1.0     | Bug in Invers(): fabs() statt ABS() (Typ SHORT !!) *
  23  *****************************************************************************/
  24
  25
  26
  27
  28 /*****************************************************************************
  29  * USHORT Invers(inv,n,p)                                                    *
  30  * Zweck     : Berechnung der inversen Matrix mit Hilfe des Gauß-Algorithmus *
  31  * Input     : inv    - Matrix, von der die inverse Matrix berechnet werden  *
  32  *                      soll                                                 *
  33  *             n      - Größe der (n,n)-Matrix                               *
  34  * Output    : inv    - inverse Matrix                                       *
  35  *             p      - n-Permutationsvektor                                 *
  36  * Ergebnis  : INVERTABLE/NOT_INVERTABLE                                     *
  37  *****************************************************************************/
  38
  39 #include <aros/oldprograms.h>
  40 #include "simplex.h"
  41
  42 USHORT Invers(inv,n,p)
  43 DOUBLE *inv;
  44 SHORT  n,*p;
  45 {
  46   REGISTER DOUBLE *ptr1, *ptr2, *ptr3, s;
  47   REGISTER SHORT  i,j,k;
  48   REGISTER DOUBLE max;
  49
  50   for(k=1; k<=n; ++k) {
  51     max = 0.0;
  52     ptr2 = inv + ((LONG)(k-1)*(LONG)(n+1));
  53     for(i=k; i<=n; ++i) {
  54       s = 0;
  55       ptr1 = inv + ((LONG)(i-1)*(LONG)n+(LONG)(k-1));
  56       for(j=k; j<=n; ++j) {
  57         s += fabs(*ptr1);
  58         ++ptr1;
  59       }
  60       if(s==0.0) return(NOT_INVERTABLE);
  61       s = fabs(*ptr2)/s;
  62       ptr2 += n;
  63       if(s>max) {
  64         max = s;
  65         p[k-1] = i;
  66       }
  67     }
  68     if(max<EPS_INV) return(NOT_INVERTABLE);
  69     if((i=p[k-1]) != k) {
  70       ptr1 = inv+(i-1)*n;
  71       ptr2 = inv+(k-1)*n;
  72       for(j=0; j<n; ++j) {
  73         s = *ptr1;
  74         *ptr1++ = *ptr2;
  75         *ptr2++ = s;
  76       }
  77     }
  78     s = *(inv + ((LONG)(k-1)*(LONG)(n+1)));
  79     ptr1 = inv + ((LONG)(k-1)*(LONG)n);
  80     for(j=1; j<=n; ++j) {
  81       if(j!=k) {
  82         *ptr1 = -(*ptr1)/s;
  83         ptr2 = inv + (j-1);
  84         ptr3 = inv + (k-1);
  85         for(i=1; i<=n; ++i) {
  86           if(i!=k) *ptr2 += (*ptr3)*(*ptr1);
  87           ptr2 += n;
  88           ptr3 += n;
  89         }
  90       }
  91       ++ptr1;
  92     }
  93     ptr1 = inv + (k-1);
  94     for(i=1; i<=n; ++i) {
  95       if(i!=k) *ptr1 /= s;
  96       else     *ptr1 = 1.0/s;
  97       ptr1 += n;
  98     }
  99   }
 100
 101
 102   for(k=n-1; k>0; --k) {
 103     if((j=p[k-1]) != k) {
 104       ptr1 = inv + (k-1);
 105       ptr2 = inv + (j-1);
 106       for(i=1; i<=n; ++i) {
 107         s = *ptr1;
 108         *ptr1 = *ptr2;
 109         *ptr2 = s;
 110         ptr1 += n;
 111         ptr2 += n;
 112       }
 113     }
 114   }
 115
 116 return(INVERTABLE);
 117 }
 118
 119
 120
 121 /*****************************************************************************
 122  * VOID Mult(A,B,erg,n,m,k)                                                  *
 123  * Zweck     : Matrizenmultiplikation erg = A*B                              *
 124  * Input     : A      - (n,m)-Matrix                                         *
 125  *             B      - (m,k)-Matrix                                         *
 126  *             n,m,k  - Zeilen- und Spaltenanzahlen                          *
 127  * Output    : erg    - (n,k)-Matrix A*B                                     *
 128  *****************************************************************************/
 129
 130 VOID    Mult(A,B,erg,n,m,k)
 131 DOUBLE  *A,*B,*erg;
 132 SHORT   n,m,k;
 133 {
 134   REGISTER DOUBLE *Aptr,*Bptr,s;
 135   REGISTER SHORT  h,j,i;
 136
 137   for(i=0; i<n; ++i) {          /* Zeilen von erg  */
 138     for(j=0; j<k; ++j) {        /* Spalten von erg */
 139       Aptr = A;
 140       Bptr = B;
 141       for(s=0.0,h=0; h<m; ++h) {
 142         s += (*Aptr++)*(*Bptr);
 143         Bptr += k;
 144       }
 145       *erg++ = s;
 146       ++B;
 147     }
 148     A += m;
 149     B -= k;
 150   }
 151 }
 152
 153
 154
 155 /*****************************************************************************
 156  * DOUBLE M(matrix,n,i,j,flag)                                               *
 157  * Zweck     : M() liest die Komponente a[i,j] einer Matrix                  *
 158  * Input     : matrix - Zeiger auf die Matrix                                *
 159  *             n      - Anzahl Spalten                                       *
 160  *             i      - Zeilenindex                                          *
 161  *             j      - Spaltenindex                                         *
 162  *             flag   - PHASE_I/PHASE_II                                     *
 163  * Bemerkung : Falls j>n und flag==PHASE_I, gibt M() Werte der angehängten   *
 164  *             Einheitsmatrix aus (0 oder 1)                                 *
 165  *****************************************************************************/
 166
 167 DOUBLE M(matrix,n,i,j,flag)
 168 DOUBLE *matrix;
 169 SHORT  n,i,j;
 170 USHORT flag;
 171 {
 172   if(flag==PHASE_I && j>n) return(i==j-n ? 1.0 : 0.0);
 173   else                     return(*(matrix+((LONG)(i-1)*(LONG)n+(LONG)(j-1))));
 174 }
 175
 176
 177
 178 /*****************************************************************************
 179  * VOID SetM(matrix,n,i,j,value)                                             *
 180  * Zweck     : SetM() setzt die Komponente a[i,j] einer Matrix auf 'value'   *
 181  * Input     : matrix - Zeiger auf die Matrix                                *
 182  *             n      - Anzahl Spalten                                       *
 183  *             i      - Zeilenindex                                          *
 184  *             j      - Spaltenindex                                         *
 185  *             value  - a[i,j] = value                                       *
 186  *****************************************************************************/
 187
 188 VOID    SetM(matrix,n,i,j,value)
 189 DOUBLE  *matrix;
 190 SHORT   n,i,j;
 191 DOUBLE  value;
 192 {
 193   *(matrix+((LONG)(i-1)*(LONG)n+(LONG)(j-1))) = value;
 194 }