1*ce9c7ef7SAndrew Rist /**************************************************************
2cdf0e10cSrcweir *
3*ce9c7ef7SAndrew Rist * Licensed to the Apache Software Foundation (ASF) under one
4*ce9c7ef7SAndrew Rist * or more contributor license agreements. See the NOTICE file
5*ce9c7ef7SAndrew Rist * distributed with this work for additional information
6*ce9c7ef7SAndrew Rist * regarding copyright ownership. The ASF licenses this file
7*ce9c7ef7SAndrew Rist * to you under the Apache License, Version 2.0 (the
8*ce9c7ef7SAndrew Rist * "License"); you may not use this file except in compliance
9*ce9c7ef7SAndrew Rist * with the License. You may obtain a copy of the License at
10*ce9c7ef7SAndrew Rist *
11*ce9c7ef7SAndrew Rist * http://www.apache.org/licenses/LICENSE-2.0
12*ce9c7ef7SAndrew Rist *
13*ce9c7ef7SAndrew Rist * Unless required by applicable law or agreed to in writing,
14*ce9c7ef7SAndrew Rist * software distributed under the License is distributed on an
15*ce9c7ef7SAndrew Rist * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
16*ce9c7ef7SAndrew Rist * KIND, either express or implied. See the License for the
17*ce9c7ef7SAndrew Rist * specific language governing permissions and limitations
18*ce9c7ef7SAndrew Rist * under the License.
19*ce9c7ef7SAndrew Rist *
20*ce9c7ef7SAndrew Rist *************************************************************/
21*ce9c7ef7SAndrew Rist
22*ce9c7ef7SAndrew Rist
23cdf0e10cSrcweir
24cdf0e10cSrcweir /** This method eliminates elements below main diagonal in the given
25cdf0e10cSrcweir matrix by gaussian elimination.
26cdf0e10cSrcweir
27cdf0e10cSrcweir @param matrix
28cdf0e10cSrcweir The matrix to operate on. Last column is the result vector (right
29cdf0e10cSrcweir hand side of the linear equation). After successful termination,
30cdf0e10cSrcweir the matrix is upper triangular. The matrix is expected to be in
31cdf0e10cSrcweir row major order.
32cdf0e10cSrcweir
33cdf0e10cSrcweir @param rows
34cdf0e10cSrcweir Number of rows in matrix
35cdf0e10cSrcweir
36cdf0e10cSrcweir @param cols
37cdf0e10cSrcweir Number of columns in matrix
38cdf0e10cSrcweir
39cdf0e10cSrcweir @param minPivot
40cdf0e10cSrcweir If the pivot element gets lesser than minPivot, this method fails,
41cdf0e10cSrcweir otherwise, elimination succeeds and true is returned.
42cdf0e10cSrcweir
43cdf0e10cSrcweir @return true, if elimination succeeded.
44cdf0e10cSrcweir */
45cdf0e10cSrcweir template <class Matrix, typename BaseType>
eliminate(Matrix & matrix,int rows,int cols,const BaseType & minPivot)46cdf0e10cSrcweir bool eliminate( Matrix& matrix,
47cdf0e10cSrcweir int rows,
48cdf0e10cSrcweir int cols,
49cdf0e10cSrcweir const BaseType& minPivot )
50cdf0e10cSrcweir {
51cdf0e10cSrcweir BaseType temp;
52cdf0e10cSrcweir int max, i, j, k; /* *must* be signed, when looping like: j>=0 ! */
53cdf0e10cSrcweir
54cdf0e10cSrcweir /* eliminate below main diagonal */
55cdf0e10cSrcweir for(i=0; i<cols-1; ++i)
56cdf0e10cSrcweir {
57cdf0e10cSrcweir /* find best pivot */
58cdf0e10cSrcweir max = i;
59cdf0e10cSrcweir for(j=i+1; j<rows; ++j)
60cdf0e10cSrcweir if( fabs(matrix[ j*cols + i ]) > fabs(matrix[ max*cols + i ]) )
61cdf0e10cSrcweir max = j;
62cdf0e10cSrcweir
63cdf0e10cSrcweir /* check pivot value */
64cdf0e10cSrcweir if( fabs(matrix[ max*cols + i ]) < minPivot )
65cdf0e10cSrcweir return false; /* pivot too small! */
66cdf0e10cSrcweir
67cdf0e10cSrcweir /* interchange rows 'max' and 'i' */
68cdf0e10cSrcweir for(k=0; k<cols; ++k)
69cdf0e10cSrcweir {
70cdf0e10cSrcweir temp = matrix[ i*cols + k ];
71cdf0e10cSrcweir matrix[ i*cols + k ] = matrix[ max*cols + k ];
72cdf0e10cSrcweir matrix[ max*cols + k ] = temp;
73cdf0e10cSrcweir }
74cdf0e10cSrcweir
75cdf0e10cSrcweir /* eliminate column */
76cdf0e10cSrcweir for(j=i+1; j<rows; ++j)
77cdf0e10cSrcweir for(k=cols-1; k>=i; --k)
78cdf0e10cSrcweir matrix[ j*cols + k ] -= matrix[ i*cols + k ] *
79cdf0e10cSrcweir matrix[ j*cols + i ] / matrix[ i*cols + i ];
80cdf0e10cSrcweir }
81cdf0e10cSrcweir
82cdf0e10cSrcweir /* everything went well */
83cdf0e10cSrcweir return true;
84cdf0e10cSrcweir }
85cdf0e10cSrcweir
86cdf0e10cSrcweir
87cdf0e10cSrcweir /** Retrieve solution vector of linear system by substituting backwards.
88cdf0e10cSrcweir
89cdf0e10cSrcweir This operation _relies_ on the previous successful
90cdf0e10cSrcweir application of eliminate()!
91cdf0e10cSrcweir
92cdf0e10cSrcweir @param matrix
93cdf0e10cSrcweir Matrix in upper diagonal form, as e.g. generated by eliminate()
94cdf0e10cSrcweir
95cdf0e10cSrcweir @param rows
96cdf0e10cSrcweir Number of rows in matrix
97cdf0e10cSrcweir
98cdf0e10cSrcweir @param cols
99cdf0e10cSrcweir Number of columns in matrix
100cdf0e10cSrcweir
101cdf0e10cSrcweir @param result
102cdf0e10cSrcweir Result vector. Given matrix must have space for one column (rows entries).
103cdf0e10cSrcweir
104cdf0e10cSrcweir @return true, if back substitution was possible (i.e. no division
105cdf0e10cSrcweir by zero occured).
106cdf0e10cSrcweir */
107cdf0e10cSrcweir template <class Matrix, class Vector, typename BaseType>
substitute(const Matrix & matrix,int rows,int cols,Vector & result)108cdf0e10cSrcweir bool substitute( const Matrix& matrix,
109cdf0e10cSrcweir int rows,
110cdf0e10cSrcweir int cols,
111cdf0e10cSrcweir Vector& result )
112cdf0e10cSrcweir {
113cdf0e10cSrcweir BaseType temp;
114cdf0e10cSrcweir int j,k; /* *must* be signed, when looping like: j>=0 ! */
115cdf0e10cSrcweir
116cdf0e10cSrcweir /* substitute backwards */
117cdf0e10cSrcweir for(j=rows-1; j>=0; --j)
118cdf0e10cSrcweir {
119cdf0e10cSrcweir temp = 0.0;
120cdf0e10cSrcweir for(k=j+1; k<cols-1; ++k)
121cdf0e10cSrcweir temp += matrix[ j*cols + k ] * result[k];
122cdf0e10cSrcweir
123cdf0e10cSrcweir if( matrix[ j*cols + j ] == 0.0 )
124cdf0e10cSrcweir return false; /* imminent division by zero! */
125cdf0e10cSrcweir
126cdf0e10cSrcweir result[j] = (matrix[ j*cols + cols-1 ] - temp) / matrix[ j*cols + j ];
127cdf0e10cSrcweir }
128cdf0e10cSrcweir
129cdf0e10cSrcweir /* everything went well */
130cdf0e10cSrcweir return true;
131cdf0e10cSrcweir }
132cdf0e10cSrcweir
133cdf0e10cSrcweir
134cdf0e10cSrcweir /** This method determines solution of given linear system, if any
135cdf0e10cSrcweir
136cdf0e10cSrcweir This is a wrapper for eliminate and substitute, given matrix must
137cdf0e10cSrcweir contain right side of equation as the last column.
138cdf0e10cSrcweir
139cdf0e10cSrcweir @param matrix
140cdf0e10cSrcweir The matrix to operate on. Last column is the result vector (right
141cdf0e10cSrcweir hand side of the linear equation). After successful termination,
142cdf0e10cSrcweir the matrix is upper triangular. The matrix is expected to be in
143cdf0e10cSrcweir row major order.
144cdf0e10cSrcweir
145cdf0e10cSrcweir @param rows
146cdf0e10cSrcweir Number of rows in matrix
147cdf0e10cSrcweir
148cdf0e10cSrcweir @param cols
149cdf0e10cSrcweir Number of columns in matrix
150cdf0e10cSrcweir
151cdf0e10cSrcweir @param minPivot
152cdf0e10cSrcweir If the pivot element gets lesser than minPivot, this method fails,
153cdf0e10cSrcweir otherwise, elimination succeeds and true is returned.
154cdf0e10cSrcweir
155cdf0e10cSrcweir @return true, if elimination succeeded.
156cdf0e10cSrcweir */
157cdf0e10cSrcweir template <class Matrix, class Vector, typename BaseType>
solve(Matrix & matrix,int rows,int cols,Vector & result,BaseType minPivot)158cdf0e10cSrcweir bool solve( Matrix& matrix,
159cdf0e10cSrcweir int rows,
160cdf0e10cSrcweir int cols,
161cdf0e10cSrcweir Vector& result,
162cdf0e10cSrcweir BaseType minPivot )
163cdf0e10cSrcweir {
164cdf0e10cSrcweir if( eliminate<Matrix,BaseType>(matrix, rows, cols, minPivot) )
165cdf0e10cSrcweir return substitute<Matrix,Vector,BaseType>(matrix, rows, cols, result);
166cdf0e10cSrcweir
167cdf0e10cSrcweir return false;
168cdf0e10cSrcweir }
169