Camera tracking integration
[blender.git] / extern / Eigen3 / Eigen / src / Sparse / SparseMatrix.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.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_SPARSEMATRIX_H
26 #define EIGEN_SPARSEMATRIX_H
27
28 /** \ingroup Sparse_Module
29   *
30   * \class SparseMatrix
31   *
32   * \brief The main sparse matrix class
33   *
34   * This class implements a sparse matrix using the very common compressed row/column storage
35   * scheme.
36   *
37   * \tparam _Scalar the scalar type, i.e. the type of the coefficients
38   * \tparam _Options Union of bit flags controlling the storage scheme. Currently the only possibility
39   *                 is RowMajor. The default is 0 which means column-major.
40   * \tparam _Index the type of the indices. Default is \c int.
41   *
42   * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
43   *
44   * This class can be extended with the help of the plugin mechanism described on the page
45   * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_SPARSEMATRIX_PLUGIN.
46   */
47
48 namespace internal {
49 template<typename _Scalar, int _Options, typename _Index>
50 struct traits<SparseMatrix<_Scalar, _Options, _Index> >
51 {
52   typedef _Scalar Scalar;
53   typedef _Index Index;
54   typedef Sparse StorageKind;
55   typedef MatrixXpr XprKind;
56   enum {
57     RowsAtCompileTime = Dynamic,
58     ColsAtCompileTime = Dynamic,
59     MaxRowsAtCompileTime = Dynamic,
60     MaxColsAtCompileTime = Dynamic,
61     Flags = _Options | NestByRefBit | LvalueBit,
62     CoeffReadCost = NumTraits<Scalar>::ReadCost,
63     SupportedAccessPatterns = InnerRandomAccessPattern
64   };
65 };
66
67 } // end namespace internal
68
69 template<typename _Scalar, int _Options, typename _Index>
70 class SparseMatrix
71   : public SparseMatrixBase<SparseMatrix<_Scalar, _Options, _Index> >
72 {
73   public:
74     EIGEN_SPARSE_PUBLIC_INTERFACE(SparseMatrix)
75 //     using Base::operator=;
76     EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseMatrix, +=)
77     EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseMatrix, -=)
78     // FIXME: why are these operator already alvailable ???
79     // EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(SparseMatrix, *=)
80     // EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(SparseMatrix, /=)
81
82     typedef MappedSparseMatrix<Scalar,Flags> Map;
83     using Base::IsRowMajor;
84     typedef CompressedStorage<Scalar,Index> Storage;
85     enum {
86       Options = _Options
87     };
88
89   protected:
90
91     typedef SparseMatrix<Scalar,(Flags&~RowMajorBit)|(IsRowMajor?RowMajorBit:0)> TransposedSparseMatrix;
92
93     Index m_outerSize;
94     Index m_innerSize;
95     Index* m_outerIndex;
96     CompressedStorage<Scalar,Index> m_data;
97
98   public:
99
100     inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
101     inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
102
103     inline Index innerSize() const { return m_innerSize; }
104     inline Index outerSize() const { return m_outerSize; }
105     inline Index innerNonZeros(Index j) const { return m_outerIndex[j+1]-m_outerIndex[j]; }
106
107     inline const Scalar* _valuePtr() const { return &m_data.value(0); }
108     inline Scalar* _valuePtr() { return &m_data.value(0); }
109
110     inline const Index* _innerIndexPtr() const { return &m_data.index(0); }
111     inline Index* _innerIndexPtr() { return &m_data.index(0); }
112
113     inline const Index* _outerIndexPtr() const { return m_outerIndex; }
114     inline Index* _outerIndexPtr() { return m_outerIndex; }
115
116     inline Storage& data() { return m_data; }
117     inline const Storage& data() const { return m_data; }
118
119     inline Scalar coeff(Index row, Index col) const
120     {
121       const Index outer = IsRowMajor ? row : col;
122       const Index inner = IsRowMajor ? col : row;
123       return m_data.atInRange(m_outerIndex[outer], m_outerIndex[outer+1], inner);
124     }
125
126     inline Scalar& coeffRef(Index row, Index col)
127     {
128       const Index outer = IsRowMajor ? row : col;
129       const Index inner = IsRowMajor ? col : row;
130
131       Index start = m_outerIndex[outer];
132       Index end = m_outerIndex[outer+1];
133       eigen_assert(end>=start && "you probably called coeffRef on a non finalized matrix");
134       eigen_assert(end>start && "coeffRef cannot be called on a zero coefficient");
135       const Index p = m_data.searchLowerIndex(start,end-1,inner);
136       eigen_assert((p<end) && (m_data.index(p)==inner) && "coeffRef cannot be called on a zero coefficient");
137       return m_data.value(p);
138     }
139
140   public:
141
142     class InnerIterator;
143
144     /** Removes all non zeros */
145     inline void setZero()
146     {
147       m_data.clear();
148       memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(Index));
149     }
150
151     /** \returns the number of non zero coefficients */
152     inline Index nonZeros() const  { return static_cast<Index>(m_data.size()); }
153
154     /** Preallocates \a reserveSize non zeros */
155     inline void reserve(Index reserveSize)
156     {
157       m_data.reserve(reserveSize);
158     }
159
160     //--- low level purely coherent filling ---
161
162     /** \returns a reference to the non zero coefficient at position \a row, \a col assuming that:
163       * - the nonzero does not already exist
164       * - the new coefficient is the last one according to the storage order
165       *
166       * Before filling a given inner vector you must call the statVec(Index) function.
167       *
168       * After an insertion session, you should call the finalize() function.
169       *
170       * \sa insert, insertBackByOuterInner, startVec */
171     inline Scalar& insertBack(Index row, Index col)
172     {
173       return insertBackByOuterInner(IsRowMajor?row:col, IsRowMajor?col:row);
174     }
175
176     /** \sa insertBack, startVec */
177     inline Scalar& insertBackByOuterInner(Index outer, Index inner)
178     {
179       eigen_assert(size_t(m_outerIndex[outer+1]) == m_data.size() && "Invalid ordered insertion (invalid outer index)");
180       eigen_assert( (m_outerIndex[outer+1]-m_outerIndex[outer]==0 || m_data.index(m_data.size()-1)<inner) && "Invalid ordered insertion (invalid inner index)");
181       Index p = m_outerIndex[outer+1];
182       ++m_outerIndex[outer+1];
183       m_data.append(0, inner);
184       return m_data.value(p);
185     }
186
187     /** \warning use it only if you know what you are doing */
188     inline Scalar& insertBackByOuterInnerUnordered(Index outer, Index inner)
189     {
190       Index p = m_outerIndex[outer+1];
191       ++m_outerIndex[outer+1];
192       m_data.append(0, inner);
193       return m_data.value(p);
194     }
195
196     /** \sa insertBack, insertBackByOuterInner */
197     inline void startVec(Index outer)
198     {
199       eigen_assert(m_outerIndex[outer]==int(m_data.size()) && "You must call startVec for each inner vector sequentially");
200       eigen_assert(m_outerIndex[outer+1]==0 && "You must call startVec for each inner vector sequentially");
201       m_outerIndex[outer+1] = m_outerIndex[outer];
202     }
203
204     //---
205
206     /** \returns a reference to a novel non zero coefficient with coordinates \a row x \a col.
207       * The non zero coefficient must \b not already exist.
208       *
209       * \warning This function can be extremely slow if the non zero coefficients
210       * are not inserted in a coherent order.
211       *
212       * After an insertion session, you should call the finalize() function.
213       */
214     EIGEN_DONT_INLINE Scalar& insert(Index row, Index col)
215     {
216       const Index outer = IsRowMajor ? row : col;
217       const Index inner = IsRowMajor ? col : row;
218
219       Index previousOuter = outer;
220       if (m_outerIndex[outer+1]==0)
221       {
222         // we start a new inner vector
223         while (previousOuter>=0 && m_outerIndex[previousOuter]==0)
224         {
225           m_outerIndex[previousOuter] = static_cast<Index>(m_data.size());
226           --previousOuter;
227         }
228         m_outerIndex[outer+1] = m_outerIndex[outer];
229       }
230
231       // here we have to handle the tricky case where the outerIndex array
232       // starts with: [ 0 0 0 0 0 1 ...] and we are inserting in, e.g.,
233       // the 2nd inner vector...
234       bool isLastVec = (!(previousOuter==-1 && m_data.size()!=0))
235                     && (size_t(m_outerIndex[outer+1]) == m_data.size());
236
237       size_t startId = m_outerIndex[outer];
238       // FIXME let's make sure sizeof(long int) == sizeof(size_t)
239       size_t p = m_outerIndex[outer+1];
240       ++m_outerIndex[outer+1];
241
242       float reallocRatio = 1;
243       if (m_data.allocatedSize()<=m_data.size())
244       {
245         // if there is no preallocated memory, let's reserve a minimum of 32 elements
246         if (m_data.size()==0)
247         {
248           m_data.reserve(32);
249         }
250         else
251         {
252           // we need to reallocate the data, to reduce multiple reallocations
253           // we use a smart resize algorithm based on the current filling ratio
254           // in addition, we use float to avoid integers overflows
255           float nnzEstimate = float(m_outerIndex[outer])*float(m_outerSize)/float(outer+1);
256           reallocRatio = (nnzEstimate-float(m_data.size()))/float(m_data.size());
257           // furthermore we bound the realloc ratio to:
258           //   1) reduce multiple minor realloc when the matrix is almost filled
259           //   2) avoid to allocate too much memory when the matrix is almost empty
260           reallocRatio = (std::min)((std::max)(reallocRatio,1.5f),8.f);
261         }
262       }
263       m_data.resize(m_data.size()+1,reallocRatio);
264
265       if (!isLastVec)
266       {
267         if (previousOuter==-1)
268         {
269           // oops wrong guess.
270           // let's correct the outer offsets
271           for (Index k=0; k<=(outer+1); ++k)
272             m_outerIndex[k] = 0;
273           Index k=outer+1;
274           while(m_outerIndex[k]==0)
275             m_outerIndex[k++] = 1;
276           while (k<=m_outerSize && m_outerIndex[k]!=0)
277             m_outerIndex[k++]++;
278           p = 0;
279           --k;
280           k = m_outerIndex[k]-1;
281           while (k>0)
282           {
283             m_data.index(k) = m_data.index(k-1);
284             m_data.value(k) = m_data.value(k-1);
285             k--;
286           }
287         }
288         else
289         {
290           // we are not inserting into the last inner vec
291           // update outer indices:
292           Index j = outer+2;
293           while (j<=m_outerSize && m_outerIndex[j]!=0)
294             m_outerIndex[j++]++;
295           --j;
296           // shift data of last vecs:
297           Index k = m_outerIndex[j]-1;
298           while (k>=Index(p))
299           {
300             m_data.index(k) = m_data.index(k-1);
301             m_data.value(k) = m_data.value(k-1);
302             k--;
303           }
304         }
305       }
306
307       while ( (p > startId) && (m_data.index(p-1) > inner) )
308       {
309         m_data.index(p) = m_data.index(p-1);
310         m_data.value(p) = m_data.value(p-1);
311         --p;
312       }
313
314       m_data.index(p) = inner;
315       return (m_data.value(p) = 0);
316     }
317
318
319
320
321     /** Must be called after inserting a set of non zero entries.
322       */
323     inline void finalize()
324     {
325       Index size = static_cast<Index>(m_data.size());
326       Index i = m_outerSize;
327       // find the last filled column
328       while (i>=0 && m_outerIndex[i]==0)
329         --i;
330       ++i;
331       while (i<=m_outerSize)
332       {
333         m_outerIndex[i] = size;
334         ++i;
335       }
336     }
337
338     /** Suppress all nonzeros which are smaller than \a reference under the tolerence \a epsilon */
339     void prune(Scalar reference, RealScalar epsilon = NumTraits<RealScalar>::dummy_precision())
340     {
341       prune(default_prunning_func(reference,epsilon));
342     }
343     
344     /** Suppress all nonzeros which do not satisfy the predicate \a keep.
345       * The functor type \a KeepFunc must implement the following function:
346       * \code
347       * bool operator() (const Index& row, const Index& col, const Scalar& value) const;
348       * \endcode
349       * \sa prune(Scalar,RealScalar)
350       */
351     template<typename KeepFunc>
352     void prune(const KeepFunc& keep = KeepFunc())
353     {
354       Index k = 0;
355       for(Index j=0; j<m_outerSize; ++j)
356       {
357         Index previousStart = m_outerIndex[j];
358         m_outerIndex[j] = k;
359         Index end = m_outerIndex[j+1];
360         for(Index i=previousStart; i<end; ++i)
361         {
362           if(keep(IsRowMajor?j:m_data.index(i), IsRowMajor?m_data.index(i):j, m_data.value(i)))
363           {
364             m_data.value(k) = m_data.value(i);
365             m_data.index(k) = m_data.index(i);
366             ++k;
367           }
368         }
369       }
370       m_outerIndex[m_outerSize] = k;
371       m_data.resize(k,0);
372     }
373
374     /** Resizes the matrix to a \a rows x \a cols matrix and initializes it to zero
375       * \sa resizeNonZeros(Index), reserve(), setZero()
376       */
377     void resize(Index rows, Index cols)
378     {
379       const Index outerSize = IsRowMajor ? rows : cols;
380       m_innerSize = IsRowMajor ? cols : rows;
381       m_data.clear();
382       if (m_outerSize != outerSize || m_outerSize==0)
383       {
384         delete[] m_outerIndex;
385         m_outerIndex = new Index [outerSize+1];
386         m_outerSize = outerSize;
387       }
388       memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(Index));
389     }
390
391     /** Low level API
392       * Resize the nonzero vector to \a size */
393     void resizeNonZeros(Index size)
394     {
395       m_data.resize(size);
396     }
397
398     /** Default constructor yielding an empty \c 0 \c x \c 0 matrix */
399     inline SparseMatrix()
400       : m_outerSize(-1), m_innerSize(0), m_outerIndex(0)
401     {
402       resize(0, 0);
403     }
404
405     /** Constructs a \a rows \c x \a cols empty matrix */
406     inline SparseMatrix(Index rows, Index cols)
407       : m_outerSize(0), m_innerSize(0), m_outerIndex(0)
408     {
409       resize(rows, cols);
410     }
411
412     /** Constructs a sparse matrix from the sparse expression \a other */
413     template<typename OtherDerived>
414     inline SparseMatrix(const SparseMatrixBase<OtherDerived>& other)
415       : m_outerSize(0), m_innerSize(0), m_outerIndex(0)
416     {
417       *this = other.derived();
418     }
419
420     /** Copy constructor */
421     inline SparseMatrix(const SparseMatrix& other)
422       : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0)
423     {
424       *this = other.derived();
425     }
426
427     /** Swap the content of two sparse matrices of same type (optimization) */
428     inline void swap(SparseMatrix& other)
429     {
430       //EIGEN_DBG_SPARSE(std::cout << "SparseMatrix:: swap\n");
431       std::swap(m_outerIndex, other.m_outerIndex);
432       std::swap(m_innerSize, other.m_innerSize);
433       std::swap(m_outerSize, other.m_outerSize);
434       m_data.swap(other.m_data);
435     }
436
437     inline SparseMatrix& operator=(const SparseMatrix& other)
438     {
439 //       std::cout << "SparseMatrix& operator=(const SparseMatrix& other)\n";
440       if (other.isRValue())
441       {
442         swap(other.const_cast_derived());
443       }
444       else
445       {
446         resize(other.rows(), other.cols());
447         memcpy(m_outerIndex, other.m_outerIndex, (m_outerSize+1)*sizeof(Index));
448         m_data = other.m_data;
449       }
450       return *this;
451     }
452
453     #ifndef EIGEN_PARSED_BY_DOXYGEN
454     template<typename Lhs, typename Rhs>
455     inline SparseMatrix& operator=(const SparseSparseProduct<Lhs,Rhs>& product)
456     { return Base::operator=(product); }
457     
458     template<typename OtherDerived>
459     inline SparseMatrix& operator=(const ReturnByValue<OtherDerived>& other)
460     { return Base::operator=(other); }
461     
462     template<typename OtherDerived>
463     inline SparseMatrix& operator=(const EigenBase<OtherDerived>& other)
464     { return Base::operator=(other); }
465     #endif
466
467     template<typename OtherDerived>
468     EIGEN_DONT_INLINE SparseMatrix& operator=(const SparseMatrixBase<OtherDerived>& other)
469     {
470       const bool needToTranspose = (Flags & RowMajorBit) != (OtherDerived::Flags & RowMajorBit);
471       if (needToTranspose)
472       {
473         // two passes algorithm:
474         //  1 - compute the number of coeffs per dest inner vector
475         //  2 - do the actual copy/eval
476         // Since each coeff of the rhs has to be evaluated twice, let's evaluate it if needed
477         typedef typename internal::nested<OtherDerived,2>::type OtherCopy;
478         typedef typename internal::remove_all<OtherCopy>::type _OtherCopy;
479         OtherCopy otherCopy(other.derived());
480
481         resize(other.rows(), other.cols());
482         Eigen::Map<Matrix<Index, Dynamic, 1> > (m_outerIndex,outerSize()).setZero();
483         // pass 1
484         // FIXME the above copy could be merged with that pass
485         for (Index j=0; j<otherCopy.outerSize(); ++j)
486           for (typename _OtherCopy::InnerIterator it(otherCopy, j); it; ++it)
487             ++m_outerIndex[it.index()];
488
489         // prefix sum
490         Index count = 0;
491         VectorXi positions(outerSize());
492         for (Index j=0; j<outerSize(); ++j)
493         {
494           Index tmp = m_outerIndex[j];
495           m_outerIndex[j] = count;
496           positions[j] = count;
497           count += tmp;
498         }
499         m_outerIndex[outerSize()] = count;
500         // alloc
501         m_data.resize(count);
502         // pass 2
503         for (Index j=0; j<otherCopy.outerSize(); ++j)
504         {
505           for (typename _OtherCopy::InnerIterator it(otherCopy, j); it; ++it)
506           {
507             Index pos = positions[it.index()]++;
508             m_data.index(pos) = j;
509             m_data.value(pos) = it.value();
510           }
511         }
512         return *this;
513       }
514       else
515       {
516         // there is no special optimization
517         return SparseMatrixBase<SparseMatrix>::operator=(other.derived());
518       }
519     }
520
521     friend std::ostream & operator << (std::ostream & s, const SparseMatrix& m)
522     {
523       EIGEN_DBG_SPARSE(
524         s << "Nonzero entries:\n";
525         for (Index i=0; i<m.nonZeros(); ++i)
526         {
527           s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
528         }
529         s << std::endl;
530         s << std::endl;
531         s << "Column pointers:\n";
532         for (Index i=0; i<m.outerSize(); ++i)
533         {
534           s << m.m_outerIndex[i] << " ";
535         }
536         s << " $" << std::endl;
537         s << std::endl;
538       );
539       s << static_cast<const SparseMatrixBase<SparseMatrix>&>(m);
540       return s;
541     }
542
543     /** Destructor */
544     inline ~SparseMatrix()
545     {
546       delete[] m_outerIndex;
547     }
548
549     /** Overloaded for performance */
550     Scalar sum() const;
551
552   public:
553
554     /** \deprecated use setZero() and reserve()
555       * Initializes the filling process of \c *this.
556       * \param reserveSize approximate number of nonzeros
557       * Note that the matrix \c *this is zero-ed.
558       */
559     EIGEN_DEPRECATED void startFill(Index reserveSize = 1000)
560     {
561       setZero();
562       m_data.reserve(reserveSize);
563     }
564
565     /** \deprecated use insert()
566       * Like fill() but with random inner coordinates.
567       */
568     EIGEN_DEPRECATED Scalar& fillrand(Index row, Index col)
569     {
570       return insert(row,col);
571     }
572
573     /** \deprecated use insert()
574       */
575     EIGEN_DEPRECATED Scalar& fill(Index row, Index col)
576     {
577       const Index outer = IsRowMajor ? row : col;
578       const Index inner = IsRowMajor ? col : row;
579
580       if (m_outerIndex[outer+1]==0)
581       {
582         // we start a new inner vector
583         Index i = outer;
584         while (i>=0 && m_outerIndex[i]==0)
585         {
586           m_outerIndex[i] = m_data.size();
587           --i;
588         }
589         m_outerIndex[outer+1] = m_outerIndex[outer];
590       }
591       else
592       {
593         eigen_assert(m_data.index(m_data.size()-1)<inner && "wrong sorted insertion");
594       }
595 //       std::cerr << size_t(m_outerIndex[outer+1]) << " == " << m_data.size() << "\n";
596       assert(size_t(m_outerIndex[outer+1]) == m_data.size());
597       Index p = m_outerIndex[outer+1];
598       ++m_outerIndex[outer+1];
599
600       m_data.append(0, inner);
601       return m_data.value(p);
602     }
603
604     /** \deprecated use finalize */
605     EIGEN_DEPRECATED void endFill() { finalize(); }
606     
607 #   ifdef EIGEN_SPARSEMATRIX_PLUGIN
608 #     include EIGEN_SPARSEMATRIX_PLUGIN
609 #   endif
610
611 private:
612   struct default_prunning_func {
613     default_prunning_func(Scalar ref, RealScalar eps) : reference(ref), epsilon(eps) {}
614     inline bool operator() (const Index&, const Index&, const Scalar& value) const
615     {
616       return !internal::isMuchSmallerThan(value, reference, epsilon);
617     }
618     Scalar reference;
619     RealScalar epsilon;
620   };
621 };
622
623 template<typename Scalar, int _Options, typename _Index>
624 class SparseMatrix<Scalar,_Options,_Index>::InnerIterator
625 {
626   public:
627     InnerIterator(const SparseMatrix& mat, Index outer)
628       : m_values(mat._valuePtr()), m_indices(mat._innerIndexPtr()), m_outer(outer), m_id(mat.m_outerIndex[outer]), m_end(mat.m_outerIndex[outer+1])
629     {}
630
631     inline InnerIterator& operator++() { m_id++; return *this; }
632
633     inline const Scalar& value() const { return m_values[m_id]; }
634     inline Scalar& valueRef() { return const_cast<Scalar&>(m_values[m_id]); }
635
636     inline Index index() const { return m_indices[m_id]; }
637     inline Index outer() const { return m_outer; }
638     inline Index row() const { return IsRowMajor ? m_outer : index(); }
639     inline Index col() const { return IsRowMajor ? index() : m_outer; }
640
641     inline operator bool() const { return (m_id < m_end); }
642
643   protected:
644     const Scalar* m_values;
645     const Index* m_indices;
646     const Index m_outer;
647     Index m_id;
648     const Index m_end;
649 };
650
651 #endif // EIGEN_SPARSEMATRIX_H