Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / extern / Eigen2 / Eigen / src / Core / SolveTriangular.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 Gael Guennebaud <g.gael@free.fr>
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_SOLVETRIANGULAR_H
26 #define EIGEN_SOLVETRIANGULAR_H
27
28 template<typename XprType> struct ei_is_part { enum {value=false}; };
29 template<typename XprType, unsigned int Mode> struct ei_is_part<Part<XprType,Mode> > { enum {value=true}; };
30
31 template<typename Lhs, typename Rhs,
32   int TriangularPart = (int(Lhs::Flags) & LowerTriangularBit)
33                      ? LowerTriangular
34                      : (int(Lhs::Flags) & UpperTriangularBit)
35                      ? UpperTriangular
36                      : -1,
37   int StorageOrder = ei_is_part<Lhs>::value ? -1  // this is to solve ambiguous specializations
38                    : int(Lhs::Flags) & (RowMajorBit|SparseBit)
39   >
40 struct ei_solve_triangular_selector;
41
42 // transform a Part xpr to a Flagged xpr
43 template<typename Lhs, unsigned int LhsMode, typename Rhs, int UpLo, int StorageOrder>
44 struct ei_solve_triangular_selector<Part<Lhs,LhsMode>,Rhs,UpLo,StorageOrder>
45 {
46   static void run(const Part<Lhs,LhsMode>& lhs, Rhs& other)
47   {
48     ei_solve_triangular_selector<Flagged<Lhs,LhsMode,0>,Rhs>::run(lhs._expression(), other);
49   }
50 };
51
52 // forward substitution, row-major
53 template<typename Lhs, typename Rhs, int UpLo>
54 struct ei_solve_triangular_selector<Lhs,Rhs,UpLo,RowMajor|IsDense>
55 {
56   typedef typename Rhs::Scalar Scalar;
57   static void run(const Lhs& lhs, Rhs& other)
58   {
59     const bool IsLowerTriangular = (UpLo==LowerTriangular);
60     const int size = lhs.cols();
61     /* We perform the inverse product per block of 4 rows such that we perfectly match
62      * our optimized matrix * vector product. blockyStart represents the number of rows
63      * we have process first using the non-block version.
64      */
65     int blockyStart = (std::max(size-5,0)/4)*4;
66     if (IsLowerTriangular)
67       blockyStart = size - blockyStart;
68     else
69       blockyStart -= 1;
70     for(int c=0 ; c<other.cols() ; ++c)
71     {
72       // process first rows using the non block version
73       if(!(Lhs::Flags & UnitDiagBit))
74       {
75         if (IsLowerTriangular)
76           other.coeffRef(0,c) = other.coeff(0,c)/lhs.coeff(0, 0);
77         else
78           other.coeffRef(size-1,c) = other.coeff(size-1, c)/lhs.coeff(size-1, size-1);
79       }
80       for(int i=(IsLowerTriangular ? 1 : size-2); IsLowerTriangular ? i<blockyStart : i>blockyStart; i += (IsLowerTriangular ? 1 : -1) )
81       {
82         Scalar tmp = other.coeff(i,c)
83           - (IsLowerTriangular ? ((lhs.row(i).start(i)) * other.col(c).start(i)).coeff(0,0)
84                      : ((lhs.row(i).end(size-i-1)) * other.col(c).end(size-i-1)).coeff(0,0));
85         if (Lhs::Flags & UnitDiagBit)
86           other.coeffRef(i,c) = tmp;
87         else
88           other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
89       }
90
91       // now let's process the remaining rows 4 at once
92       for(int i=blockyStart; IsLowerTriangular ? i<size : i>0; )
93       {
94         int startBlock = i;
95         int endBlock = startBlock + (IsLowerTriangular ? 4 : -4);
96
97         /* Process the i cols times 4 rows block, and keep the result in a temporary vector */
98         // FIXME use fixed size block but take care to small fixed size matrices...
99         Matrix<Scalar,Dynamic,1> btmp(4);
100         if (IsLowerTriangular)
101           btmp = lhs.block(startBlock,0,4,i) * other.col(c).start(i);
102         else
103           btmp = lhs.block(i-3,i+1,4,size-1-i) * other.col(c).end(size-1-i);
104
105         /* Let's process the 4x4 sub-matrix as usual.
106          * btmp stores the diagonal coefficients used to update the remaining part of the result.
107          */
108         {
109           Scalar tmp = other.coeff(startBlock,c)-btmp.coeff(IsLowerTriangular?0:3);
110           if (Lhs::Flags & UnitDiagBit)
111             other.coeffRef(i,c) = tmp;
112           else
113             other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
114         }
115
116         i += IsLowerTriangular ? 1 : -1;
117         for (;IsLowerTriangular ? i<endBlock : i>endBlock; i += IsLowerTriangular ? 1 : -1)
118         {
119           int remainingSize = IsLowerTriangular ? i-startBlock : startBlock-i;
120           Scalar tmp = other.coeff(i,c)
121             - btmp.coeff(IsLowerTriangular ? remainingSize : 3-remainingSize)
122             - (   lhs.row(i).segment(IsLowerTriangular ? startBlock : i+1, remainingSize)
123               * other.col(c).segment(IsLowerTriangular ? startBlock : i+1, remainingSize)).coeff(0,0);
124
125           if (Lhs::Flags & UnitDiagBit)
126             other.coeffRef(i,c) = tmp;
127           else
128             other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
129         }
130       }
131     }
132   }
133 };
134
135 // Implements the following configurations:
136 //  - inv(LowerTriangular,         ColMajor) * Column vector
137 //  - inv(LowerTriangular,UnitDiag,ColMajor) * Column vector
138 //  - inv(UpperTriangular,         ColMajor) * Column vector
139 //  - inv(UpperTriangular,UnitDiag,ColMajor) * Column vector
140 template<typename Lhs, typename Rhs, int UpLo>
141 struct ei_solve_triangular_selector<Lhs,Rhs,UpLo,ColMajor|IsDense>
142 {
143   typedef typename Rhs::Scalar Scalar;
144   typedef typename ei_packet_traits<Scalar>::type Packet;
145   enum { PacketSize =  ei_packet_traits<Scalar>::size };
146
147   static void run(const Lhs& lhs, Rhs& other)
148   {
149     static const bool IsLowerTriangular = (UpLo==LowerTriangular);
150     const int size = lhs.cols();
151     for(int c=0 ; c<other.cols() ; ++c)
152     {
153       /* let's perform the inverse product per block of 4 columns such that we perfectly match
154        * our optimized matrix * vector product. blockyEnd represents the number of rows
155        * we can process using the block version.
156        */
157       int blockyEnd = (std::max(size-5,0)/4)*4;
158       if (!IsLowerTriangular)
159         blockyEnd = size-1 - blockyEnd;
160       for(int i=IsLowerTriangular ? 0 : size-1; IsLowerTriangular ? i<blockyEnd : i>blockyEnd;)
161       {
162         /* Let's process the 4x4 sub-matrix as usual.
163          * btmp stores the diagonal coefficients used to update the remaining part of the result.
164          */
165         int startBlock = i;
166         int endBlock = startBlock + (IsLowerTriangular ? 4 : -4);
167         Matrix<Scalar,4,1> btmp;
168         for (;IsLowerTriangular ? i<endBlock : i>endBlock;
169              i += IsLowerTriangular ? 1 : -1)
170         {
171           if(!(Lhs::Flags & UnitDiagBit))
172             other.coeffRef(i,c) /= lhs.coeff(i,i);
173           int remainingSize = IsLowerTriangular ? endBlock-i-1 : i-endBlock-1;
174           if (remainingSize>0)
175             other.col(c).segment((IsLowerTriangular ? i : endBlock) + 1, remainingSize) -=
176                 other.coeffRef(i,c)
177               * Block<Lhs,Dynamic,1>(lhs, (IsLowerTriangular ? i : endBlock) + 1, i, remainingSize, 1);
178           btmp.coeffRef(IsLowerTriangular ? i-startBlock : remainingSize) = -other.coeffRef(i,c);
179         }
180
181         /* Now we can efficiently update the remaining part of the result as a matrix * vector product.
182          * NOTE in order to reduce both compilation time and binary size, let's directly call
183          * the fast product implementation. It is equivalent to the following code:
184          *   other.col(c).end(size-endBlock) += (lhs.block(endBlock, startBlock, size-endBlock, endBlock-startBlock)
185          *                                       * other.col(c).block(startBlock,endBlock-startBlock)).lazy();
186          */
187         // FIXME this is cool but what about conjugate/adjoint expressions ? do we want to evaluate them ?
188         // this is a more general problem though.
189         ei_cache_friendly_product_colmajor_times_vector(
190           IsLowerTriangular ? size-endBlock : endBlock+1,
191           &(lhs.const_cast_derived().coeffRef(IsLowerTriangular ? endBlock : 0, IsLowerTriangular ? startBlock : endBlock+1)),
192           lhs.stride(),
193           btmp, &(other.coeffRef(IsLowerTriangular ? endBlock : 0, c)));
194 //                              if (IsLowerTriangular)
195 //           other.col(c).end(size-endBlock) += (lhs.block(endBlock, startBlock, size-endBlock, endBlock-startBlock)
196 //                                           * other.col(c).block(startBlock,endBlock-startBlock)).lazy();
197 //                              else
198 //           other.col(c).end(size-endBlock) += (lhs.block(endBlock, startBlock, size-endBlock, endBlock-startBlock)
199 //                                           * other.col(c).block(startBlock,endBlock-startBlock)).lazy();
200       }
201
202       /* Now we have to process the remaining part as usual */
203       int i;
204       for(i=blockyEnd; IsLowerTriangular ? i<size-1 : i>0; i += (IsLowerTriangular ? 1 : -1) )
205       {
206         if(!(Lhs::Flags & UnitDiagBit))
207           other.coeffRef(i,c) /= lhs.coeff(i,i);
208
209         /* NOTE we cannot use lhs.col(i).end(size-i-1) because Part::coeffRef gets called by .col() to
210          * get the address of the start of the row
211          */
212         if(IsLowerTriangular)
213           other.col(c).end(size-i-1) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, i+1,i, size-i-1,1);
214         else
215           other.col(c).start(i) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, 0,i, i, 1);
216       }
217       if(!(Lhs::Flags & UnitDiagBit))
218         other.coeffRef(i,c) /= lhs.coeff(i,i);
219     }
220   }
221 };
222
223 /** "in-place" version of MatrixBase::solveTriangular() where the result is written in \a other
224   *
225   * \nonstableyet
226   *
227   * The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
228   * This function will const_cast it, so constness isn't honored here.
229   *
230   * See MatrixBase:solveTriangular() for the details.
231   */
232 template<typename Derived>
233 template<typename OtherDerived>
234 void MatrixBase<Derived>::solveTriangularInPlace(const MatrixBase<OtherDerived>& _other) const
235 {
236   MatrixBase<OtherDerived>& other = _other.const_cast_derived();
237   ei_assert(derived().cols() == derived().rows());
238   ei_assert(derived().cols() == other.rows());
239   ei_assert(!(Flags & ZeroDiagBit));
240   ei_assert(Flags & (UpperTriangularBit|LowerTriangularBit));
241
242   enum { copy = ei_traits<OtherDerived>::Flags & RowMajorBit };
243
244   typedef typename ei_meta_if<copy,
245     typename ei_plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::ret OtherCopy;
246   OtherCopy otherCopy(other.derived());
247
248   ei_solve_triangular_selector<Derived, typename ei_unref<OtherCopy>::type>::run(derived(), otherCopy);
249
250   if (copy)
251     other = otherCopy;
252 }
253
254 /** \returns the product of the inverse of \c *this with \a other, \a *this being triangular.
255   *
256   * \nonstableyet
257   *
258   * This function computes the inverse-matrix matrix product inverse(\c *this) * \a other.
259   * The matrix \c *this must be triangular and invertible (i.e., all the coefficients of the
260   * diagonal must be non zero). It works as a forward (resp. backward) substitution if \c *this
261   * is an upper (resp. lower) triangular matrix.
262   *
263   * It is required that \c *this be marked as either an upper or a lower triangular matrix, which
264   * can be done by marked(), and that is automatically the case with expressions such as those returned
265   * by extract().
266   *
267   * \addexample SolveTriangular \label How to solve a triangular system (aka. how to multiply the inverse of a triangular matrix by another one)
268   *
269   * Example: \include MatrixBase_marked.cpp
270   * Output: \verbinclude MatrixBase_marked.out
271   *
272   * This function is essentially a wrapper to the faster solveTriangularInPlace() function creating
273   * a temporary copy of \a other, calling solveTriangularInPlace() on the copy and returning it.
274   * Therefore, if \a other is not needed anymore, it is quite faster to call solveTriangularInPlace()
275   * instead of solveTriangular().
276   *
277   * For users coming from BLAS, this function (and more specifically solveTriangularInPlace()) offer
278   * all the operations supported by the \c *TRSV and \c *TRSM BLAS routines.
279   *
280   * \b Tips: to perform a \em "right-inverse-multiply" you can simply transpose the operation, e.g.:
281   * \code
282   * M * T^1  <=>  T.transpose().solveTriangularInPlace(M.transpose());
283   * \endcode
284   *
285   * \sa solveTriangularInPlace(), marked(), extract()
286   */
287 template<typename Derived>
288 template<typename OtherDerived>
289 typename ei_plain_matrix_type_column_major<OtherDerived>::type
290 MatrixBase<Derived>::solveTriangular(const MatrixBase<OtherDerived>& other) const
291 {
292   typename ei_plain_matrix_type_column_major<OtherDerived>::type res(other);
293   solveTriangularInPlace(res);
294   return res;
295 }
296
297 #endif // EIGEN_SOLVETRIANGULAR_H