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
25namespace 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
148