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 swap_arrays(T * a1,T * a2,unsigned n)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 { pivotmatrix_pivot45 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 { solvesimul_eq81 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