#include <TrackPropagation/NavGeometry/src/LinearEquation3.h>
Public Member Functions | |
Basic3DVector< T > | solution (const Basic3DVector< T > &row0, const Basic3DVector< T > &row1, const Basic3DVector< T > &row2, const Basic3DVector< T > &rhsvec) const |
Classes | |
class | Array3 |
Definition at line 13 of file LinearEquation3.h.
Basic3DVector<T> LinearEquation3< T >::solution | ( | const Basic3DVector< T > & | row0, | |
const Basic3DVector< T > & | row1, | |||
const Basic3DVector< T > & | row2, | |||
const Basic3DVector< T > & | rhsvec | |||
) | const [inline] |
Definition at line 55 of file LinearEquation3.h.
References funct::abs(), c1, c2, c3, i1, i2, row, LinearEquation3< T >::Array3< U >::subtractScaled(), and std::swap().
Referenced by ThreePlaneCrossing::crossing().
00058 { 00059 00060 // copy the input to internal "matrix" 00061 Array3<T> row[3]; 00062 row[0] = row0; 00063 row[1] = row1; 00064 row[2] = row2; 00065 Array3<T> rhs(rhsvec); 00066 00067 // no implicit pivoting - rows expected to be normalized already 00068 00069 // find pivot 0, i.e. row with largest first element 00070 int i0 = std::abs(row[0][0]) > std::abs(row[1][0]) ? 0 : 1; 00071 if (std::abs(row[i0][0]) < std::abs(row[2][0])) i0 = 2; 00072 00073 int i1 = (i0+1)%3; 00074 int i2 = (i0+2)%3; 00075 00076 // zero the first column of rows i1 and i2 00077 T c1 = row[i1][0] / row[i0][0]; 00078 // row[i1] -= c1*row[i0]; 00079 row[i1].subtractScaled( row[i0], c1); 00080 rhs[i1] -= c1*rhs[i0]; 00081 T c2 = row[i2][0] / row[i0][0]; 00082 // row[i2] -= c2*row[i0]; 00083 row[i2].subtractScaled( row[i0], c2); 00084 rhs[i2] -= c2*rhs[i0]; 00085 00086 // find pivot 1, i.e. which row (i1 or i2) has the largest second element 00087 if (std::abs(row[i1][1]) < std::abs(row[i2][1])) std::swap( i1, i2); 00088 00089 // zero the second column of row i2 00090 T c3 = row[i2][1] / row[i1][1]; 00091 row[i2][1] -= c3 * row[i1][1]; 00092 row[i2][2] -= c3 * row[i1][2]; 00093 rhs[i2] -= c3*rhs[i1]; 00094 00095 // compute the solution 00096 T x2 = rhs[i2] / row[i2][2]; 00097 T x1 = (rhs[i1] - x2*row[i1][2]) / row[i1][1]; 00098 T x0 = (rhs[i0] - x1*row[i0][1] - x2*row[i0][2]) / row[i0][0]; 00099 00100 return Basic3DVector<T>(x0, x1, x2); 00101 }