Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / extern / Eigen2 / Eigen / src / LU / Inverse.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra. Eigen itself is part of the KDE project.
3 //
4 // Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
5 //
6 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24
25 #ifndef EIGEN_INVERSE_H
26 #define EIGEN_INVERSE_H
27
28 /********************************************************************
29 *** Part 1 : optimized implementations for fixed-size 2,3,4 cases ***
30 ********************************************************************/
31
32 template<typename MatrixType>
33 void ei_compute_inverse_in_size2_case(const MatrixType& matrix, MatrixType* result)
34 {
35   typedef typename MatrixType::Scalar Scalar;
36   const Scalar invdet = Scalar(1) / matrix.determinant();
37   result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
38   result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
39   result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
40   result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
41 }
42
43 template<typename XprType, typename MatrixType>
44 bool ei_compute_inverse_in_size2_case_with_check(const XprType& matrix, MatrixType* result)
45 {
46   typedef typename MatrixType::Scalar Scalar;
47   const Scalar det = matrix.determinant();
48   if(ei_isMuchSmallerThan(det, matrix.cwise().abs().maxCoeff())) return false;
49   const Scalar invdet = Scalar(1) / det;
50   result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
51   result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
52   result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
53   result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
54   return true;
55 }
56
57 template<typename MatrixType>
58 void ei_compute_inverse_in_size3_case(const MatrixType& matrix, MatrixType* result)
59 {
60   typedef typename MatrixType::Scalar Scalar;
61   const Scalar det_minor00 = matrix.minor(0,0).determinant();
62   const Scalar det_minor10 = matrix.minor(1,0).determinant();
63   const Scalar det_minor20 = matrix.minor(2,0).determinant();
64   const Scalar invdet = Scalar(1) / ( det_minor00 * matrix.coeff(0,0)
65                                     - det_minor10 * matrix.coeff(1,0)
66                                     + det_minor20 * matrix.coeff(2,0) );
67   result->coeffRef(0, 0) = det_minor00 * invdet;
68   result->coeffRef(0, 1) = -det_minor10 * invdet;
69   result->coeffRef(0, 2) = det_minor20 * invdet;
70   result->coeffRef(1, 0) = -matrix.minor(0,1).determinant() * invdet;
71   result->coeffRef(1, 1) = matrix.minor(1,1).determinant() * invdet;
72   result->coeffRef(1, 2) = -matrix.minor(2,1).determinant() * invdet;
73   result->coeffRef(2, 0) = matrix.minor(0,2).determinant() * invdet;
74   result->coeffRef(2, 1) = -matrix.minor(1,2).determinant() * invdet;
75   result->coeffRef(2, 2) = matrix.minor(2,2).determinant() * invdet;
76 }
77
78 template<typename MatrixType>
79 bool ei_compute_inverse_in_size4_case_helper(const MatrixType& matrix, MatrixType* result)
80 {
81   /* Let's split M into four 2x2 blocks:
82     * (P Q)
83     * (R S)
84     * If P is invertible, with inverse denoted by P_inverse, and if
85     * (S - R*P_inverse*Q) is also invertible, then the inverse of M is
86     * (P' Q')
87     * (R' S')
88     * where
89     * S' = (S - R*P_inverse*Q)^(-1)
90     * P' = P1 + (P1*Q) * S' *(R*P_inverse)
91     * Q' = -(P_inverse*Q) * S'
92     * R' = -S' * (R*P_inverse)
93     */
94   typedef Block<MatrixType,2,2> XprBlock22;
95   typedef typename MatrixBase<XprBlock22>::PlainMatrixType Block22;
96   Block22 P_inverse;
97   if(ei_compute_inverse_in_size2_case_with_check(matrix.template block<2,2>(0,0), &P_inverse))
98   {
99     const Block22 Q = matrix.template block<2,2>(0,2);
100     const Block22 P_inverse_times_Q = P_inverse * Q;
101     const XprBlock22 R = matrix.template block<2,2>(2,0);
102     const Block22 R_times_P_inverse = R * P_inverse;
103     const Block22 R_times_P_inverse_times_Q = R_times_P_inverse * Q;
104     const XprBlock22 S = matrix.template block<2,2>(2,2);
105     const Block22 X = S - R_times_P_inverse_times_Q;
106     Block22 Y;
107     ei_compute_inverse_in_size2_case(X, &Y);
108     result->template block<2,2>(2,2) = Y;
109     result->template block<2,2>(2,0) = - Y * R_times_P_inverse;
110     const Block22 Z = P_inverse_times_Q * Y;
111     result->template block<2,2>(0,2) = - Z;
112     result->template block<2,2>(0,0) = P_inverse + Z * R_times_P_inverse;
113     return true;
114   }
115   else
116   {
117     return false;
118   }
119 }
120
121 template<typename MatrixType>
122 void ei_compute_inverse_in_size4_case(const MatrixType& matrix, MatrixType* result)
123 {
124   if(ei_compute_inverse_in_size4_case_helper(matrix, result))
125   {
126     // good ! The topleft 2x2 block was invertible, so the 2x2 blocks approach is successful.
127     return;
128   }
129   else
130   {
131     // rare case: the topleft 2x2 block is not invertible (but the matrix itself is assumed to be).
132     // since this is a rare case, we don't need to optimize it. We just want to handle it with little
133     // additional code.
134     MatrixType m(matrix);
135     m.row(0).swap(m.row(2));
136     m.row(1).swap(m.row(3));
137     if(ei_compute_inverse_in_size4_case_helper(m, result))
138     {
139       // good, the topleft 2x2 block of m is invertible. Since m is different from matrix in that some
140       // rows were permuted, the actual inverse of matrix is derived from the inverse of m by permuting
141       // the corresponding columns.
142       result->col(0).swap(result->col(2));
143       result->col(1).swap(result->col(3));
144     }
145     else
146     {
147       // last possible case. Since matrix is assumed to be invertible, this last case has to work.
148       // first, undo the swaps previously made
149       m.row(0).swap(m.row(2));
150       m.row(1).swap(m.row(3));
151       // swap row 0 with the the row among 0 and 1 that has the biggest 2 first coeffs
152       int swap0with = ei_abs(m.coeff(0,0))+ei_abs(m.coeff(0,1))>ei_abs(m.coeff(1,0))+ei_abs(m.coeff(1,1)) ? 0 : 1;
153       m.row(0).swap(m.row(swap0with));
154       // swap row 1 with the the row among 2 and 3 that has the biggest 2 first coeffs
155       int swap1with = ei_abs(m.coeff(2,0))+ei_abs(m.coeff(2,1))>ei_abs(m.coeff(3,0))+ei_abs(m.coeff(3,1)) ? 2 : 3;
156       m.row(1).swap(m.row(swap1with));
157       ei_compute_inverse_in_size4_case_helper(m, result);
158       result->col(1).swap(result->col(swap1with));
159       result->col(0).swap(result->col(swap0with));
160     }
161   }
162 }
163
164 /***********************************************
165 *** Part 2 : selector and MatrixBase methods ***
166 ***********************************************/
167
168 template<typename MatrixType, int Size = MatrixType::RowsAtCompileTime>
169 struct ei_compute_inverse
170 {
171   static inline void run(const MatrixType& matrix, MatrixType* result)
172   {
173     LU<MatrixType> lu(matrix);
174     lu.computeInverse(result);
175   }
176 };
177
178 template<typename MatrixType>
179 struct ei_compute_inverse<MatrixType, 1>
180 {
181   static inline void run(const MatrixType& matrix, MatrixType* result)
182   {
183     typedef typename MatrixType::Scalar Scalar;
184     result->coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
185   }
186 };
187
188 template<typename MatrixType>
189 struct ei_compute_inverse<MatrixType, 2>
190 {
191   static inline void run(const MatrixType& matrix, MatrixType* result)
192   {
193     ei_compute_inverse_in_size2_case(matrix, result);
194   }
195 };
196
197 template<typename MatrixType>
198 struct ei_compute_inverse<MatrixType, 3>
199 {
200   static inline void run(const MatrixType& matrix, MatrixType* result)
201   {
202     ei_compute_inverse_in_size3_case(matrix, result);
203   }
204 };
205
206 template<typename MatrixType>
207 struct ei_compute_inverse<MatrixType, 4>
208 {
209   static inline void run(const MatrixType& matrix, MatrixType* result)
210   {
211     ei_compute_inverse_in_size4_case(matrix, result);
212   }
213 };
214
215 /** \lu_module
216   *
217   * Computes the matrix inverse of this matrix.
218   *
219   * \note This matrix must be invertible, otherwise the result is undefined.
220   *
221   * \param result Pointer to the matrix in which to store the result.
222   *
223   * Example: \include MatrixBase_computeInverse.cpp
224   * Output: \verbinclude MatrixBase_computeInverse.out
225   *
226   * \sa inverse()
227   */
228 template<typename Derived>
229 inline void MatrixBase<Derived>::computeInverse(PlainMatrixType *result) const
230 {
231   ei_assert(rows() == cols());
232   EIGEN_STATIC_ASSERT(NumTraits<Scalar>::HasFloatingPoint,NUMERIC_TYPE_MUST_BE_FLOATING_POINT)
233   ei_compute_inverse<PlainMatrixType>::run(eval(), result);
234 }
235
236 /** \lu_module
237   *
238   * \returns the matrix inverse of this matrix.
239   *
240   * \note This matrix must be invertible, otherwise the result is undefined.
241   *
242   * \note This method returns a matrix by value, which can be inefficient. To avoid that overhead,
243   * use computeInverse() instead.
244   *
245   * Example: \include MatrixBase_inverse.cpp
246   * Output: \verbinclude MatrixBase_inverse.out
247   *
248   * \sa computeInverse()
249   */
250 template<typename Derived>
251 inline const typename MatrixBase<Derived>::PlainMatrixType MatrixBase<Derived>::inverse() const
252 {
253   PlainMatrixType result(rows(), cols());
254   computeInverse(&result);
255   return result;
256 }
257
258 #endif // EIGEN_INVERSE_H