headers/libs/agg/agg_simul_eq.h

   1 //----------------------------------------------------------------------------
   2 // Anti-Grain Geometry - Version 2.4
   3 // Copyright (C) 2002-2005 Maxim Shemanarev (http://www.antigrain.com)
   4 //
   5 // Permission to copy, use, modify, sell and distribute this software
   6 // is granted provided this copyright notice appears in all copies.
   7 // This software is provided "as is" without express or implied
   8 // warranty, and with no claim as to its suitability for any purpose.
   9 //
  10 //----------------------------------------------------------------------------
  11 // Contact: mcseem@antigrain.com
  12 //          mcseemagg@yahoo.com
  13 //          http://www.antigrain.com
  14 //----------------------------------------------------------------------------
  15 //
  16 // Solving simultaneous equations
  17 //
  18 //----------------------------------------------------------------------------
  19 #ifndef AGG_SIMUL_EQ_INCLUDED
  20 #define AGG_SIMUL_EQ_INCLUDED
  21
  22 #include <math.h>
  23 #include "agg_basics.h"
  24
  25 namespace agg
  26 {
  27
  28     //=============================================================swap_arrays
  29     template<class T> void swap_arrays(T* a1, T* a2, unsigned n)
  30     {
  31         unsigned i;
  32         for(i = 0; i < n; i++)
  33         {
  34             T tmp = *a1;
  35             *a1++ = *a2;
  36             *a2++ = tmp;
  37         }
  38     }
  39
  40
  41     //============================================================matrix_pivot
  42     template<unsigned Rows, unsigned Cols>
  43     struct matrix_pivot
  44     {
  45         static int pivot(double m[Rows][Cols], unsigned row)
  46         {
  47             int k = int(row);
  48             double max_val, tmp;
  49
  50             max_val = -1.0;
  51             unsigned i;
  52             for(i = row; i < Rows; i++)
  53             {
  54                 if((tmp = fabs(m[i][row])) > max_val && tmp != 0.0)
  55                 {
  56                     max_val = tmp;
  57                     k = i;
  58                 }
  59             }
  60
  61             if(m[k][row] == 0.0)
  62             {
  63                 return -1;
  64             }
  65
  66             if(k != int(row))
  67             {
  68                 swap_arrays(m[k], m[row], Cols);
  69                 return k;
  70             }
  71             return 0;
  72         }
  73     };
  74
  75
  76
  77     //===============================================================simul_eq
  78     template<unsigned Size, unsigned RightCols>
  79     struct simul_eq
  80     {
  81         static bool solve(const double left[Size][Size],
  82                           const double right[Size][RightCols],
  83                           double result[Size][RightCols])
  84         {
  85             unsigned i, j, k;
  86             double a1;
  87
  88             double tmp[Size][Size + RightCols];
  89
  90             for(i = 0; i < Size; i++)
  91             {
  92                 for(j = 0; j < Size; j++)
  93                 {
  94                     tmp[i][j] = left[i][j];
  95                 }
  96                 for(j = 0; j < RightCols; j++)
  97                 {
  98                     tmp[i][Size + j] = right[i][j];
  99                 }
 100             }
 101
 102             for(k = 0; k < Size; k++)
 103             {
 104                 if(matrix_pivot<Size, Size + RightCols>::pivot(tmp, k) < 0)
 105                 {
 106                     return false; // Singularity....
 107                 }
 108
 109                 a1 = tmp[k][k];
 110
 111                 for(j = k; j < Size + RightCols; j++)
 112                 {
 113                     tmp[k][j] /= a1;
 114                 }
 115
 116                 for(i = k + 1; i < Size; i++)
 117                 {
 118                     a1 = tmp[i][k];
 119                     for (j = k; j < Size + RightCols; j++)
 120                     {
 121                         tmp[i][j] -= a1 * tmp[k][j];
 122                     }
 123                 }
 124             }
 125
 126
 127             for(k = 0; k < RightCols; k++)
 128             {
 129                 int m;
 130                 for(m = int(Size - 1); m >= 0; m--)
 131                 {
 132                     result[m][k] = tmp[m][Size + k];
 133                     for(j = m + 1; j < Size; j++)
 134                     {
 135                         result[m][k] -= tmp[m][j] * result[j][k];
 136                     }
 137                 }
 138             }
 139             return true;
 140         }
 141
 142     };
 143
 144
 145 }
 146
 147 #endif