// -*- C++ -*-
/*
Copyright (c) 2008 University of North Carolina at Chapel Hill

This file is part of SSBA (Simple Sparse Bundle Adjustment).

SSBA is free software: you can redistribute it and/or modify it under the
terms of the GNU Lesser General Public License as published by the Free
Software Foundation, either version 3 of the License, or (at your option) any
later version.

SSBA is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
details.

You should have received a copy of the GNU Lesser General Public License along
with SSBA. If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef V3D_LINEAR_H
#define V3D_LINEAR_H

#include <cassert>
#include <algorithm>
#include <vector>
#include <cmath>

namespace V3D
{
   using namespace std;

   //! Unboxed vector type
   template <typename Elem, int Size>
   struct InlineVectorBase
   {
         typedef Elem value_type;
         typedef Elem element_type;

         typedef Elem const * const_iterator;
         typedef Elem       * iterator;

         static unsigned int size() { return Size; }

         Elem& operator[](unsigned int i)       { return _vec[i]; }
         Elem  operator[](unsigned int i) const { return _vec[i]; }

         Elem& operator()(unsigned int i)       { return _vec[i-1]; }
         Elem  operator()(unsigned int i) const { return _vec[i-1]; }

         const_iterator begin() const { return _vec; }
         iterator       begin()       { return _vec; }
         const_iterator end() const { return _vec + Size; }
         iterator       end()       { return _vec + Size; }

         void newsize(unsigned int sz) const
         {
            assert(sz == Size);
         }

      protected:
         Elem _vec[Size];
   };

   //! Boxed (heap allocated) vector.
   template <typename Elem>
   struct VectorBase
   {
         typedef Elem value_type;
         typedef Elem element_type;

         typedef Elem const * const_iterator;
         typedef Elem       * iterator;

         VectorBase()
            : _size(0), _ownsVec(true), _vec(0)
         { }

         VectorBase(unsigned int size)
            : _size(size), _ownsVec(true), _vec(0)
         {
            if (size > 0) _vec = new Elem[size];
         }

         VectorBase(unsigned int size, Elem * values)
            : _size(size), _ownsVec(false), _vec(values)
         { }

         VectorBase(VectorBase<Elem> const& a)
            : _size(0), _ownsVec(true), _vec(0)
         {
            _size = a._size;
            if (_size == 0) return;
            _vec = new Elem[_size];
            std::copy(a._vec, a._vec + _size, _vec);
         }

         ~VectorBase() { if (_ownsVec && _vec != 0) delete [] _vec; }

         VectorBase& operator=(VectorBase<Elem> const& a)
         {
            if (this == &a) return *this;

            this->newsize(a._size);
            std::copy(a._vec, a._vec + _size, _vec);
            return *this;
         }

         unsigned int size() const { return _size; }

         VectorBase<Elem>& newsize(unsigned int sz)
         {
            if (sz == _size) return *this;
            assert(_ownsVec);

            __destroy();
            _size = sz;
            if (_size > 0) _vec = new Elem[_size];

            return *this;
         }


         Elem& operator[](unsigned int i)       { return _vec[i]; }
         Elem  operator[](unsigned int i) const { return _vec[i]; }

         Elem& operator()(unsigned int i)       { return _vec[i-1]; }
         Elem  operator()(unsigned int i) const { return _vec[i-1]; }

         const_iterator begin() const { return _vec; }
         iterator       begin()       { return _vec; }
         const_iterator end() const { return _vec + _size; }
         iterator       end()       { return _vec + _size; }

      protected:
         void __destroy()
         {
            assert(_ownsVec);

            if (_vec != 0) delete [] _vec;
            _size = 0;
            _vec  = 0;
         }

         unsigned int _size;
         bool         _ownsVec;
         Elem       * _vec;
   };

   template <typename Elem, int Rows, int Cols>
   struct InlineMatrixBase
   {
         typedef Elem         value_type;
         typedef Elem         element_type;

         typedef Elem       * iterator;
         typedef Elem const * const_iterator;

         static unsigned int num_rows() { return Rows; }
         static unsigned int num_cols() { return Cols; }

         Elem       * operator[](unsigned int row)        { return _m[row]; }
         Elem const * operator[](unsigned int row) const  { return _m[row]; }

         Elem& operator()(unsigned int row, unsigned int col)       { return _m[row-1][col-1]; }
         Elem  operator()(unsigned int row, unsigned int col) const { return _m[row-1][col-1]; }

         template <typename Vec>
         void getRowSlice(unsigned int row, unsigned int first, unsigned int last, Vec& dst) const
         {
            for (unsigned int c = first; c < last; ++c) dst[c-first] = _m[row][c];
         }

         template <typename Vec>
         void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const
         {
            for (unsigned int r = 0; r < len; ++r) dst[r] = _m[r+first][col];
         }

         void newsize(unsigned int rows, unsigned int cols) const
         {
            assert(rows == Rows && cols == Cols);
         }

         const_iterator begin() const { return &_m[0][0]; }
         iterator       begin()       { return &_m[0][0]; }
         const_iterator end() const { return &_m[0][0] + Rows*Cols; }
         iterator       end()       { return &_m[0][0] + Rows*Cols; }

      protected:
         Elem _m[Rows][Cols];
   };

   template <typename Elem>
   struct MatrixBase
   {
         typedef Elem value_type;
         typedef Elem element_type;

         typedef Elem const * const_iterator;
         typedef Elem       * iterator;

         MatrixBase()
            : _rows(0), _cols(0), _ownsData(true), _m(0)
         { }

         MatrixBase(unsigned int rows, unsigned int cols)
            : _rows(rows), _cols(cols), _ownsData(true), _m(0)
         {
            if (_rows * _cols == 0) return;
            _m = new Elem[rows*cols];
         }

         MatrixBase(unsigned int rows, unsigned int cols, Elem * values)
            : _rows(rows), _cols(cols), _ownsData(false), _m(values)
         { }

         MatrixBase(MatrixBase<Elem> const& a)
            : _ownsData(true), _m(0)
         {
            _rows = a._rows; _cols = a._cols;
            if (_rows * _cols == 0) return;
            _m = new Elem[_rows*_cols];
            std::copy(a._m, a._m+_rows*_cols, _m);
         }

         ~MatrixBase()
         {
            if (_ownsData && _m != 0) delete [] _m;
         }

         MatrixBase& operator=(MatrixBase<Elem> const& a)
         {
            if (this == &a) return *this;

            this->newsize(a.num_rows(), a.num_cols());

            std::copy(a._m, a._m+_rows*_cols, _m);
            return *this;
         }

         void newsize(unsigned int rows, unsigned int cols)
         {
            if (rows == _rows && cols == _cols) return;

            assert(_ownsData);

            __destroy();

            _rows = rows;
            _cols = cols;
            if (_rows * _cols == 0) return;
            _m = new Elem[rows*cols];
         }

         unsigned int num_rows() const { return _rows; }
         unsigned int num_cols() const { return _cols; }

         Elem       * operator[](unsigned int row)        { return _m + row*_cols; }
         Elem const * operator[](unsigned int row) const  { return _m + row*_cols; }

         Elem& operator()(unsigned int row, unsigned int col)       { return _m[(row-1)*_cols + col-1]; }
         Elem  operator()(unsigned int row, unsigned int col) const { return _m[(row-1)*_cols + col-1]; }

         const_iterator begin() const { return _m; }
         iterator       begin()       { return _m; }
         const_iterator end() const { return _m + _rows*_cols; }
         iterator       end()       { return _m + _rows*_cols; }

         template <typename Vec>
         void getRowSlice(unsigned int row, unsigned int first, unsigned int last, Vec& dst) const
         {
            Elem const * v = (*this)[row];
            for (unsigned int c = first; c < last; ++c) dst[c-first] = v[c];
         }

         template <typename Vec>
         void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const
         {
            for (unsigned int r = 0; r < len; ++r) dst[r] = _m[r+first][col];
         }

      protected:
         void __destroy()
         {
            assert(_ownsData);
            if (_m != 0) delete [] _m;
            _m = 0;
            _rows = _cols = 0;
         }

         unsigned int   _rows, _cols;
         bool           _ownsData;
         Elem         * _m;
   };

   template <typename T>
   struct CCS_Matrix
   {
         CCS_Matrix()
            : _rows(0), _cols(0)
         { }

         CCS_Matrix(int const rows, int const cols, vector<pair<int, int> > const& nonZeros)
            : _rows(rows), _cols(cols)
         {
            this->initialize(nonZeros);
         }

         CCS_Matrix(CCS_Matrix const& b)
            : _rows(b._rows), _cols(b._cols),
              _colStarts(b._colStarts), _rowIdxs(b._rowIdxs), _destIdxs(b._destIdxs), _values(b._values)
         { }

         CCS_Matrix& operator=(CCS_Matrix const& b)
         {
            if (this == &b) return *this;
            _rows       = b._rows;
            _cols       = b._cols;
            _colStarts  = b._colStarts;
            _rowIdxs    = b._rowIdxs;
            _destIdxs   = b._destIdxs;
            _values     = b._values;
            return *this;
         }

         void create(int const rows, int const cols, vector<pair<int, int> > const& nonZeros)
         {
            _rows = rows;
            _cols = cols;
            this->initialize(nonZeros);
         }

         unsigned int num_rows() const { return _rows; }
         unsigned int num_cols() const { return _cols; }

         int getNonzeroCount() const { return _values.size(); }

         T const * getValues() const { return &_values[0]; }
         T       * getValues()       { return &_values[0]; }

         int const * getDestIndices()  const { return &_destIdxs[0]; }
         int const * getColumnStarts() const { return &_colStarts[0]; }
         int const * getRowIndices()   const { return &_rowIdxs[0]; }

         void getRowRange(unsigned int col, unsigned int& firstRow, unsigned int& lastRow) const
         {
            firstRow = _rowIdxs[_colStarts[col]];
            lastRow  = _rowIdxs[_colStarts[col+1]-1]+1;
         }

         template <typename Vec>
         void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const
         {
            unsigned int const last = first + len;

            for (int r = 0; r < len; ++r) dst[r] = 0; // Fill vector with zeros

            int const colStart = _colStarts[col];
            int const colEnd   = _colStarts[col+1];

            int i = colStart;
            int r;
            // Skip rows less than the given start row
            while (i < colEnd && (r = _rowIdxs[i]) < first) ++i;

            // Copy elements until the final row
            while (i < colEnd && (r = _rowIdxs[i]) < last)
            {
               dst[r-first] = _values[i];
               ++i;
            }
         } // end getColumnSlice()

         int getColumnNonzeroCount(unsigned int col) const
         {
            int const colStart = _colStarts[col];
            int const colEnd   = _colStarts[col+1];
            return colEnd - colStart;
         }

         template <typename VecA, typename VecB>
         void getSparseColumn(unsigned int col, VecA& rows, VecB& values) const
         {
            int const colStart = _colStarts[col];
            int const colEnd   = _colStarts[col+1];
            int const nnz      = colEnd - colStart;

            for (int i = 0; i < nnz; ++i)
            {
               rows[i] = _rowIdxs[colStart + i];
               values[i] = _values[colStart + i];
            }
         }

      protected:
         struct NonzeroInfo
         {
               int row, col, serial;

               // Sort wrt the column first
               bool operator<(NonzeroInfo const& rhs) const
               {
                  if (col < rhs.col) return true;
                  if (col > rhs.col) return false;
                  return row < rhs.row;
               }
         };

         void initialize(std::vector<std::pair<int, int> > const& nonZeros)
         {
            using namespace std;

            int const nnz = nonZeros.size();

            _colStarts.resize(_cols + 1);
            _rowIdxs.resize(nnz);

            vector<NonzeroInfo> nz(nnz);
            for (int k = 0; k < nnz; ++k)
            {
               nz[k].row    = nonZeros[k].first;
               nz[k].col    = nonZeros[k].second;
               nz[k].serial = k;
            }

            // Sort in column major order
            std::sort(nz.begin(), nz.end());

            for (size_t k = 0; k < nnz; ++k) _rowIdxs[k] = nz[k].row;

            int curCol = -1;
            for (int k = 0; k < nnz; ++k)
            {
               NonzeroInfo const& el = nz[k];
               if (el.col != curCol)
               {
                  // Update empty cols between
                  for (int c = curCol+1; c < el.col; ++c) _colStarts[c] = k;

                  curCol = el.col;
                  _colStarts[curCol] = k;
               } // end if
            } // end for (k)

            // Update remaining columns
            for (int c = curCol+1; c <= _cols; ++c) _colStarts[c] = nnz;

            _destIdxs.resize(nnz);
            for (int k = 0; k < nnz; ++k) _destIdxs[nz[k].serial] = k;

            _values.resize(nnz);
         } // end initialize()

         int              _rows, _cols;
         std::vector<int> _colStarts;
         std::vector<int> _rowIdxs;
         std::vector<int> _destIdxs;
         std::vector<T>   _values;
   }; // end struct CCS_Matrix

//----------------------------------------------------------------------

   template <typename Vec, typename Elem>
   inline void
   fillVector(Vec& v, Elem val)
   {
      // We do not use std::fill since we rely only on size() and operator[] member functions.
      for (unsigned int i = 0; i < v.size(); ++i) v[i] = val;
   }

   template <typename Vec>
   inline void
   makeZeroVector(Vec& v)
   {
      fillVector(v, 0);
   }

   template <typename VecA, typename VecB>
   inline void
   copyVector(VecA const& src, VecB& dst)
   {
      assert(src.size() == dst.size());
      // We do not use std::fill since we rely only on size() and operator[] member functions.
      for (unsigned int i = 0; i < src.size(); ++i) dst[i] = src[i];
   }

   template <typename VecA, typename VecB>
   inline void
   copyVectorSlice(VecA const& src, unsigned int srcStart, unsigned int srcLen,
                   VecB& dst, unsigned int dstStart)
   {
      unsigned int const end = std::min(srcStart + srcLen, src.size());
      unsigned int const sz = dst.size();
      unsigned int i0, i1;
      for (i0 = srcStart, i1 = dstStart; i0 < end && i1 < sz; ++i0, ++i1) dst[i1] = src[i0];
   }

   template <typename Vec>
   inline typename Vec::value_type
   norm_L1(Vec const& v)
   {
      typename Vec::value_type res(0);
      for (unsigned int i = 0; i < v.size(); ++i) res += fabs(v[i]);
      return res;
   }

   template <typename Vec>
   inline typename Vec::value_type
   norm_Linf(Vec const& v)
   {
      typename Vec::value_type res(0);
      for (unsigned int i = 0; i < v.size(); ++i) res = std::max(res, fabs(v[i]));
      return res;
   }

   template <typename Vec>
   inline typename Vec::value_type
   norm_L2(Vec const& v)
   {
      typename Vec::value_type res(0);
      for (unsigned int i = 0; i < v.size(); ++i) res += v[i]*v[i];
      return sqrt((double)res);
   }

   template <typename Vec>
   inline typename Vec::value_type
   sqrNorm_L2(Vec const& v)
   {
      typename Vec::value_type res(0);
      for (unsigned int i = 0; i < v.size(); ++i) res += v[i]*v[i];
      return res;
   }

   template <typename Vec>
   inline void
   normalizeVector(Vec& v)
   {
      typename Vec::value_type norm(norm_L2(v));
      for (unsigned int i = 0; i < v.size(); ++i) v[i] /= norm;
   }

   template<typename VecA, typename VecB>
   inline typename VecA::value_type
   innerProduct(VecA const& a, VecB const& b)
   {
      assert(a.size() == b.size());
      typename VecA::value_type res(0);
      for (unsigned int i = 0; i < a.size(); ++i) res += a[i] * b[i];
      return res;
   }

   template <typename Elem, typename VecA, typename VecB>
   inline void
   scaleVector(Elem s, VecA const& v, VecB& dst)
   {
      for (unsigned int i = 0; i < v.size(); ++i) dst[i] = s * v[i];
   }

   template <typename Elem, typename Vec>
   inline void
   scaleVectorIP(Elem s, Vec& v)
   {
      typedef typename Vec::value_type Elem2;
      for (unsigned int i = 0; i < v.size(); ++i)
         v[i] = (Elem2)(v[i] * s);
   }

   template <typename VecA, typename VecB, typename VecC>
   inline void
   makeCrossProductVector(VecA const& v, VecB const& w, VecC& dst)
   {
      assert(v.size() == 3);
      assert(w.size() == 3);
      assert(dst.size() == 3);
      dst[0] = v[1]*w[2] - v[2]*w[1];
      dst[1] = v[2]*w[0] - v[0]*w[2];
      dst[2] = v[0]*w[1] - v[1]*w[0];
   }

   template <typename VecA, typename VecB, typename VecC>
   inline void
   addVectors(VecA const& v, VecB const& w, VecC& dst)
   {
      assert(v.size() == w.size());
      assert(v.size() == dst.size());
      for (unsigned int i = 0; i < v.size(); ++i) dst[i] = v[i] + w[i];
   }

   template <typename VecA, typename VecB, typename VecC>
   inline void
   subtractVectors(VecA const& v, VecB const& w, VecC& dst)
   {
      assert(v.size() == w.size());
      assert(v.size() == dst.size());
      for (unsigned int i = 0; i < v.size(); ++i) dst[i] = v[i] - w[i];
   }

   template <typename MatA, typename MatB>
   inline void
   copyMatrix(MatA const& src, MatB& dst)
   {
      unsigned int const rows = src.num_rows();
      unsigned int const cols = src.num_cols();
      assert(dst.num_rows() == rows);
      assert(dst.num_cols() == cols);
      for (unsigned int c = 0; c < cols; ++c)
         for (unsigned int r = 0; r < rows; ++r) dst[r][c] = src[r][c];
   }

   template <typename MatA, typename MatB>
   inline void
   copyMatrixSlice(MatA const& src, unsigned int rowStart, unsigned int colStart, unsigned int rowLen, unsigned int colLen,
                   MatB& dst, unsigned int dstRow, unsigned int dstCol)
   {
      unsigned int const rows = dst.num_rows();
      unsigned int const cols = dst.num_cols();

      unsigned int const rowEnd = std::min(rowStart + rowLen, src.num_rows());
      unsigned int const colEnd = std::min(colStart + colLen, src.num_cols());

      unsigned int c0, c1, r0, r1;

      for (c0 = colStart, c1 = dstCol; c0 < colEnd && c1 < cols; ++c0, ++c1)
         for (r0 = rowStart, r1 = dstRow; r0 < rowEnd && r1 < rows; ++r0, ++r1)
            dst[r1][c1] = src[r0][c0];
   }

   template <typename MatA, typename MatB>
   inline void
   makeTransposedMatrix(MatA const& src, MatB& dst)
   {
      unsigned int const rows = src.num_rows();
      unsigned int const cols = src.num_cols();
      assert(dst.num_cols() == rows);
      assert(dst.num_rows() == cols);
      for (unsigned int c = 0; c < cols; ++c)
         for (unsigned int r = 0; r < rows; ++r) dst[c][r] = src[r][c];
   }

   template <typename Mat>
   inline void
   fillMatrix(Mat& m, typename Mat::value_type val)
   {
      unsigned int const rows = m.num_rows();
      unsigned int const cols = m.num_cols();
      for (unsigned int c = 0; c < cols; ++c)
         for (unsigned int r = 0; r < rows; ++r) m[r][c] = val;
   }

   template <typename Mat>
   inline void
   makeZeroMatrix(Mat& m)
   {
      fillMatrix(m, 0);
   }

   template <typename Mat>
   inline void
   makeIdentityMatrix(Mat& m)
   {
      makeZeroMatrix(m);
      unsigned int const rows = m.num_rows();
      unsigned int const cols = m.num_cols();
      unsigned int n = std::min(rows, cols);
      for (unsigned int i = 0; i < n; ++i)
         m[i][i] = 1;
   }

   template <typename Mat, typename Vec>
   inline void
   makeCrossProductMatrix(Vec const& v, Mat& m)
   {
      assert(v.size() == 3);
      assert(m.num_rows() == 3);
      assert(m.num_cols() == 3);
      m[0][0] = 0;     m[0][1] = -v[2]; m[0][2] = v[1];
      m[1][0] = v[2];  m[1][1] = 0;     m[1][2] = -v[0];
      m[2][0] = -v[1]; m[2][1] = v[0];  m[2][2] = 0;
   }

   template <typename Mat, typename Vec>
   inline void
   makeOuterProductMatrix(Vec const& v, Mat& m)
   {
      assert(m.num_cols() == m.num_rows());
      assert(v.size() == m.num_cols());
      unsigned const sz = v.size();
      for (unsigned r = 0; r < sz; ++r)
         for (unsigned c = 0; c < sz; ++c) m[r][c] = v[r]*v[c];
   }

   template <typename Mat, typename VecA, typename VecB>
   inline void
   makeOuterProductMatrix(VecA const& u, VecB const& v, Mat& m)
   {
      assert(m.num_cols() == m.num_rows());
      assert(u.size() == m.num_cols());
      assert(v.size() == m.num_cols());
      unsigned const sz = u.size();
      for (unsigned r = 0; r < sz; ++r)
         for (unsigned c = 0; c < sz; ++c) m[r][c] = u[r]*v[c];
   }

   template <typename MatA, typename MatB, typename MatC>
   void addMatrices(MatA const& a, MatB const& b, MatC& dst)
   {
      assert(a.num_cols() == b.num_cols());
      assert(a.num_rows() == b.num_rows());
      assert(dst.num_cols() == a.num_cols());
      assert(dst.num_rows() == a.num_rows());

      unsigned int const rows = a.num_rows();
      unsigned int const cols = a.num_cols();

      for (unsigned r = 0; r < rows; ++r)
         for (unsigned c = 0; c < cols; ++c) dst[r][c] = a[r][c] + b[r][c];
   }

   template <typename MatA, typename MatB>
   void addMatricesIP(MatA const& a, MatB& dst)
   {
      assert(dst.num_cols() == a.num_cols());
      assert(dst.num_rows() == a.num_rows());

      unsigned int const rows = a.num_rows();
      unsigned int const cols = a.num_cols();

      for (unsigned r = 0; r < rows; ++r)
         for (unsigned c = 0; c < cols; ++c) dst[r][c] += a[r][c];
   }

   template <typename MatA, typename MatB, typename MatC>
   void subtractMatrices(MatA const& a, MatB const& b, MatC& dst)
   {
      assert(a.num_cols() == b.num_cols());
      assert(a.num_rows() == b.num_rows());
      assert(dst.num_cols() == a.num_cols());
      assert(dst.num_rows() == a.num_rows());

      unsigned int const rows = a.num_rows();
      unsigned int const cols = a.num_cols();

      for (unsigned r = 0; r < rows; ++r)
         for (unsigned c = 0; c < cols; ++c) dst[r][c] = a[r][c] - b[r][c];
   }

   template <typename MatA, typename Elem, typename MatB>
   inline void
   makeScaledMatrix(MatA const& m, Elem scale, MatB& dst)
   {
      unsigned int const rows = m.num_rows();
      unsigned int const cols = m.num_cols();
      for (unsigned int c = 0; c < cols; ++c)
         for (unsigned int r = 0; r < rows; ++r) dst[r][c] = m[r][c] * scale;
   }

   template <typename Mat, typename Elem>
   inline void
   scaleMatrixIP(Elem scale, Mat& m)
   {
      unsigned int const rows = m.num_rows();
      unsigned int const cols = m.num_cols();
      for (unsigned int c = 0; c < cols; ++c)
         for (unsigned int r = 0; r < rows; ++r) m[r][c] *= scale;
   }

   template <typename Mat, typename VecA, typename VecB>
   inline void
   multiply_A_v(Mat const& m, VecA const& in, VecB& dst)
   {
      unsigned int const rows = m.num_rows();
      unsigned int const cols = m.num_cols();
      assert(in.size() == cols);
      assert(dst.size() == rows);

      makeZeroVector(dst);

      for (unsigned int r = 0; r < rows; ++r)
         for (unsigned int c = 0; c < cols; ++c) dst[r] += m[r][c] * in[c];
   }

   template <typename Mat, typename VecA, typename VecB>
   inline void
   multiply_A_v_projective(Mat const& m, VecA const& in, VecB& dst)
   {
      unsigned int const rows = m.num_rows();
      unsigned int const cols = m.num_cols();
      assert(in.size() == cols-1);
      assert(dst.size() == rows-1);

      typename VecB::value_type w = m(rows-1, cols-1);
      unsigned int r, i;
      for (i = 0; i < cols-1; ++i) w += m(rows-1, i) * in[i];
      for (r = 0; r < rows-1; ++r) dst[r] = m(r, cols-1);
      for (r = 0; r < rows-1; ++r)
         for (unsigned int c = 0; c < cols-1; ++c) dst[r] += m[r][c] * in[c];
      for (i = 0; i < rows-1; ++i) dst[i] /= w;
   }

   template <typename Mat, typename VecA, typename VecB>
   inline void
   multiply_A_v_affine(Mat const& m, VecA const& in, VecB& dst)
   {
      unsigned int const rows = m.num_rows();
      unsigned int const cols = m.num_cols();
      assert(in.size() == cols-1);
      assert(dst.size() == rows);

      unsigned int r;

      for (r = 0; r < rows; ++r) dst[r] = m(r, cols-1);
      for (r = 0; r < rows; ++r)
         for (unsigned int c = 0; c < cols-1; ++c) dst[r] += m[r][c] * in[c];
   }

   template <typename Mat, typename VecA, typename VecB>
   inline void
   multiply_At_v(Mat const& m, VecA const& in, VecB& dst)
   {
      unsigned int const rows = m.num_rows();
      unsigned int const cols = m.num_cols();
      assert(in.size() == rows);
      assert(dst.size() == cols);

      makeZeroVector(dst);
      for (unsigned int c = 0; c < cols; ++c)
         for (unsigned int r = 0; r < rows; ++r) dst[c] += m[r][c] * in[r];
   }

   template <typename MatA, typename MatB>
   inline void
   multiply_At_A(MatA const& a, MatB& dst)
   {
      assert(dst.num_rows() == a.num_cols());
      assert(dst.num_cols() == a.num_cols());

      typedef typename MatB::value_type Elem;

      Elem accum;
      for (unsigned int r = 0; r < a.num_cols(); ++r)
         for (unsigned int c = 0; c < a.num_cols(); ++c)
         {
            accum = 0;
            for (unsigned int k = 0; k < a.num_rows(); ++k) accum += a[k][r] * a[k][c];
            dst[r][c] = accum;
         }
   }

   template <typename MatA, typename MatB, typename MatC>
   inline void
   multiply_A_B(MatA const& a, MatB const& b, MatC& dst)
   {
      assert(a.num_cols() == b.num_rows());
      assert(dst.num_rows() == a.num_rows());
      assert(dst.num_cols() == b.num_cols());

      typedef typename MatC::value_type Elem;

      Elem accum;
      for (unsigned int r = 0; r < a.num_rows(); ++r)
         for (unsigned int c = 0; c < b.num_cols(); ++c)
         {
            accum = 0;
            for (unsigned int k = 0; k < a.num_cols(); ++k) accum += a[r][k] * b[k][c];
            dst[r][c] = accum;
         }
   }

   template <typename MatA, typename MatB, typename MatC>
   inline void
   multiply_At_B(MatA const& a, MatB const& b, MatC& dst)
   {
      assert(a.num_rows() == b.num_rows());
      assert(dst.num_rows() == a.num_cols());
      assert(dst.num_cols() == b.num_cols());

      typedef typename MatC::value_type Elem;

      Elem accum;
      for (unsigned int r = 0; r < a.num_cols(); ++r)
         for (unsigned int c = 0; c < b.num_cols(); ++c)
         {
            accum = 0;
            for (unsigned int k = 0; k < a.num_rows(); ++k) accum += a[k][r] * b[k][c];
            dst[r][c] = accum;
         }
   }

   template <typename MatA, typename MatB, typename MatC>
   inline void
   multiply_A_Bt(MatA const& a, MatB const& b, MatC& dst)
   {
      assert(a.num_cols() == b.num_cols());
      assert(dst.num_rows() == a.num_rows());
      assert(dst.num_cols() == b.num_rows());

      typedef typename MatC::value_type Elem;

      Elem accum;
      for (unsigned int r = 0; r < a.num_rows(); ++r)
         for (unsigned int c = 0; c < b.num_rows(); ++c)
         {
            accum = 0;
            for (unsigned int k = 0; k < a.num_cols(); ++k) accum += a[r][k] * b[c][k];
            dst[r][c] = accum;
         }
   }

   template <typename Mat>
   inline void
   transposeMatrixIP(Mat& a)
   {
      assert(a.num_rows() == a.num_cols());

      for (unsigned int r = 0; r < a.num_rows(); ++r)
         for (unsigned int c = 0; c < r; ++c)
            std::swap(a[r][c], a[c][r]);
   }

} // end namespace V3D

#endif