kmatrix_impl.hpp

Go to the documentation of this file.

// This file is part of kfilter.
// kfilter is a C++ variable-dimension extended kalman filter library.
//
// Copyright (C) 2004        Vincent Zalzal, Sylvain Marleau
// Copyright (C) 2001, 2004  Richard Gourdeau
// Copyright (C) 2004        GRPR and DGE's Automation sector
//                           �cole Polytechnique de Montr�al
//
// Code adapted from algorithms presented in :
//      Bierman, G. J. "Factorization Methods for Discrete Sequential
//      Estimation", Academic Press, 1977.
//
// This library 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 2.1 of the License, or (at your option) any later version.
//
// This library 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 this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#ifndef KMATRIX_IMPL_HPP
#define KMATRIX_IMPL_HPP


#include KSSTREAM_HEADER

namespace Kalman {


  class KMatrixContextImpl {
  public:


    KMatrixContextImpl(std::string elemDelim = " ",
                       std::string rowDelim = "\n",
                       std::string startDelim = std::string(),
                       std::string endDelim = std::string(),
                       unsigned prec = 4) 
      : elemDelim_(elemDelim), rowDelim_(rowDelim), startDelim_(startDelim), 
        endDelim_(endDelim), precision_(prec), width_(8+prec) {
      
      std::string ws(" \t\n");
      skipElemDelim_  = 
        ( elemDelim_.find_first_not_of(ws) != std::string::npos);
      skipRowDelim_   = 
        (  rowDelim_.find_first_not_of(ws) != std::string::npos);
      skipStartDelim_ = 
        (startDelim_.find_first_not_of(ws) != std::string::npos);
      skipEndDelim_   = 
        (  endDelim_.find_first_not_of(ws) != std::string::npos);
    }

    std::string elemDelim_;  
    std::string rowDelim_;   
    std::string startDelim_; 
    std::string endDelim_;   
    unsigned precision_;     
    unsigned width_;         
    bool skipElemDelim_;     
    bool skipRowDelim_;      
    bool skipStartDelim_;    
    bool skipEndDelim_;      
  };


  extern KMatrixContextImpl* currentMatrixContext;

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline void KMatrix<T, BEG, DBG>::init(K_UINT_32 m, K_UINT_32 n) {

    if (m != 0 && n != 0) {

      // non-empty matrix
      vimpl_.resize(m);
      T* ptr = &Mimpl_[0] - BEG;
      T** M = &vimpl_[0];
      T** end = M + m;
      
      while (M != end) {
        *M++ = ptr;
        ptr += n;
      }

      M_ = &vimpl_[0] - BEG;
      m_ = m;
      n_ = n;

    } else {

      // empty matrix
      M_ = 0;
      m_ = 0;
      n_ = 0;

    }

  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KMatrix<T, BEG, DBG>::KMatrix() {
    init(0, 0);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KMatrix<T, BEG, DBG>::KMatrix(K_UINT_32 m, K_UINT_32 n)
    : Mimpl_(m*n) { 
    init(m, n);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KMatrix<T, BEG, DBG>::KMatrix(K_UINT_32 m, K_UINT_32 n, const T& a)
    : Mimpl_(m*n, a) {
    init(m, n);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KMatrix<T, BEG, DBG>::KMatrix(K_UINT_32 m, K_UINT_32 n, const T* v)
    : Mimpl_(v, v + m*n) {
    init(m, n);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KMatrix<T, BEG, DBG>::KMatrix(const KMatrix& M) 
    : Mimpl_(M.Mimpl_) {
    init(M.m_, M.n_);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KMatrix<T, BEG, DBG>::~KMatrix() {}

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline T& KMatrix<T, BEG, DBG>::operator()(K_UINT_32 i, 
                                             K_UINT_32 j) {
    if (DBG) {
                
                if(BEG!=0)
                {
//                      if (i < BEG || i >= m_ + BEG || j < BEG || j >= n_ + BEG)
//                      {
//                              KOSTRINGSTREAM oss;
//                              oss << "Trying to access element (" << i << ", " << j 
//                                      << ") not included in [" << BEG << ", " << m_ + BEG - 1 << "][" 
//                                      << BEG << ", " << n_ + BEG - 1 << "]." KEND_OF_STREAM;
//                              throw OutOfBoundError(oss.str());
//                      }
                }else{
                        if (i >= m_ + BEG || j >= n_ + BEG)
                        {
                                KOSTRINGSTREAM oss;
                                oss << "Trying to access element (" << i << ", " << j 
                                        << ") not included in [" << BEG << ", " << m_ + BEG - 1 << "][" 
                                        << BEG << ", " << n_ + BEG - 1 << "]." KEND_OF_STREAM;
                                throw OutOfBoundError(oss.str());
                        }
                }
    }
    return M_[i][j];
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline const T& KMatrix<T, BEG, DBG>::operator()(K_UINT_32 i, 
                                                   K_UINT_32 j) const {
    if (DBG) {
                if(BEG!=0)
                {
//                      if (i < BEG || i >= m_ + BEG || j < BEG || j >= n_ + BEG) {
//                              KOSTRINGSTREAM oss;
//                              oss << "Trying to access element (" << i << ", " << j 
//                                      << ") not included in [" << BEG << ", " << m_ + BEG - 1 << "][" 
//                                      << BEG << ", " << n_ + BEG - 1 << "]." KEND_OF_STREAM;
//                              throw OutOfBoundError(oss.str());
//                      }
                }else
                {
                        if (i >= m_ + BEG || j >= n_ + BEG) {
                                KOSTRINGSTREAM oss;
                                oss << "Trying to access element (" << i << ", " << j 
                                        << ") not included in [" << BEG << ", " << m_ + BEG - 1 << "][" 
                                        << BEG << ", " << n_ + BEG - 1 << "]." KEND_OF_STREAM;
                                throw OutOfBoundError(oss.str());
                        }       
                }
    }
    return M_[i][j];
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline K_UINT_32 KMatrix<T, BEG, DBG>::nrow() const {
    return m_;
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline K_UINT_32 KMatrix<T, BEG, DBG>::ncol() const {
    return n_;
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline void KMatrix<T, BEG, DBG>::resize(K_UINT_32 m, K_UINT_32 n) {

    if (m == m_ && n == n_) {
      return;
    }

    Mimpl_.resize(m*n);
    init(m, n);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KMatrix<T, BEG, DBG>& KMatrix<T, BEG, DBG>::operator=(const T& a) {
    
    T* ptr = &Mimpl_[0];
    const T* end = ptr + Mimpl_.size();

    while (ptr != end) {
      *ptr++ = a;
    }
    return *this;
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KMatrix<T, BEG, DBG>& 
  KMatrix<T, BEG, DBG>::operator=(const KMatrix& M) {
    KMatrix temp(M);
    swap(temp);    
    return *this;
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline void KMatrix<T, BEG, DBG>::assign(K_UINT_32 m, K_UINT_32 n, 
                                           const T* v) {
    KMatrix temp(m, n, v);
    swap(temp);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline void KMatrix<T, BEG, DBG>::swap(KMatrix& M) {
    vimpl_.swap(M.vimpl_);
    Mimpl_.swap(M.Mimpl_);
    Util::swap(M_, M.M_);
    Util::swap(m_, M.m_);
    Util::swap(n_, M.n_);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline void KMatrix<T, BEG, DBG>::get(std::istream& is) {

    T* ptr = &Mimpl_[0];
    std::string tmp;
    K_UINT_32 i, j;

    if (currentMatrixContext->skipStartDelim_) {
      is >> tmp;
    }

    if (currentMatrixContext->skipRowDelim_) {

      if (currentMatrixContext->skipElemDelim_) {

        for (i = 0; i < m_-1; ++i) {
          for (j = 0; j < n_; ++j) {
            is >> *ptr++ >> tmp;
          }
        }

        for (j = 0; j < n_-1; ++j) {
          is >> *ptr++ >> tmp;
        }

        is >> *ptr;

      } else {

        for (i = 0; i < m_-1; ++i) {
          for (j = 0; j < n_; ++j) {
            is >> *ptr++;
          }
          is >> tmp;
        }

        for (j = 0; j < n_; ++j) {
          is >> *ptr++;
        }

      }

    } else {

      if (currentMatrixContext->skipElemDelim_) {

        for (i = 0; i < m_; ++i) {
          for (j = 0; j < n_-1; ++j) {
            is >> *ptr++ >> tmp;
          }
          is >> *ptr++;
        }

      } else {

        for (i = 0; i < m_; ++i) {
          for (j = 0; j < n_; ++j) {
            is >> *ptr++;
          }
        }

      }

    }

    if (currentMatrixContext->skipEndDelim_) {
      is >> tmp;
    }
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline void KMatrix<T, BEG, DBG>::put(std::ostream& os) const {
    if (m_ == 0 || n_ == 0) {
      return;
    }

    const T* ptr = &Mimpl_[0];
    K_UINT_32 i, j;

    std::ios::fmtflags f = os.setf(std::ios::scientific, std::ios::floatfield);
    os.setf(std::ios::showpoint);
    std::streamsize p = os.precision(currentMatrixContext->precision_);

    os << currentMatrixContext->startDelim_;

    for (i = 0; i < m_-1; ++i) {
      for (j = 0; j < n_-1; ++j) {
        os.width(currentMatrixContext->width_);
        os << *ptr++ << currentMatrixContext->elemDelim_;
      }
      os.width(currentMatrixContext->width_);
      os << *ptr++ << currentMatrixContext->rowDelim_;
    }

    for (j = 0; j < n_-1; ++j) {
      os.width(currentMatrixContext->width_);
      os << *ptr++ << currentMatrixContext->elemDelim_;
    }

    os.width(currentMatrixContext->width_);
    os << *ptr++ << currentMatrixContext->endDelim_;

    os.precision(p);
    os.flags(f);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline std::istream& operator>>(std::istream& is, 
                                  KMatrix<T, BEG, DBG>& M) {
    M.get(is);
    return is;
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline std::ostream& operator<<(std::ostream& os, 
                                  const KMatrix<T, BEG, DBG>& M) {
    M.put(os);
    return os;
  }

}

#endif