kvector_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 KVECTOR_IMPL_HPP
#define KVECTOR_IMPL_HPP


#include KSSTREAM_HEADER

namespace Kalman {


  class KVectorContextImpl {
  public:


    explicit KVectorContextImpl(std::string elemDelim = " ",
                                std::string startDelim = std::string(),
                                std::string endDelim = std::string(),
                                unsigned prec = 4) 
      : elemDelim_(elemDelim), startDelim_(startDelim), 
        endDelim_(endDelim), precision_(prec), width_(8+prec) {
      
      std::string ws(" \t\n");
      skipElemDelim_  = 
        ( elemDelim_.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 startDelim_; 
    std::string endDelim_;   
    unsigned precision_;     
    unsigned width_;         
    bool skipElemDelim_;     
    bool skipStartDelim_;    
    bool skipEndDelim_;      
};


  extern KVectorContextImpl* currentVectorContext;

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KVector<T, BEG, DBG>::KVector()
    : v_(0), n_(0) {}

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KVector<T, BEG, DBG>::KVector(K_UINT_32 n)
    : vimpl_(n), v_( (n != 0) ? &vimpl_[0] - BEG : 0 ), n_(n) {}

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KVector<T, BEG, DBG>::KVector(K_UINT_32 n, const T& a)
    : vimpl_(n, a), v_( (n != 0) ? &vimpl_[0] - BEG : 0 ), n_(n) {}

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KVector<T, BEG, DBG>::KVector(K_UINT_32 n, const T* v)
    : vimpl_(v, v + n), v_( (n != 0) ? &vimpl_[0] - BEG : 0 ), n_(n) {}

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KVector<T, BEG, DBG>::KVector(const KVector& v) 
    : vimpl_(v.vimpl_), v_( (v.size() != 0) ? &vimpl_[0] - BEG : 0 ), 
      n_(v.n_) {}

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

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

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

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

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline void KVector<T, BEG, DBG>::resize(K_UINT_32 n) {
    
    if (n == n_) {
      return;
    }
    
    vimpl_.resize(n);
    v_ = (n != 0) ? &vimpl_[0] - BEG : 0;
    n_ = n;
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline KVector<T, BEG, DBG>& KVector<T, BEG, DBG>::operator=(const T& a) {
    if (n_ == 0) {
      return *this;
    }

    T* ptr = &vimpl_[0];
    const T* end = ptr + n_;

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

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

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

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline void KVector<T, BEG, DBG>::swap(KVector& v) {
    vimpl_.swap(v.vimpl_);
    Util::swap(v_, v.v_);
    Util::swap(n_, v.n_);
  }

  template<typename T, K_UINT_32 BEG, bool DBG>
  inline void KVector<T, BEG, DBG>::get(std::istream& is) {
    if (n_ == 0) {
      return;
    }
    
    T* ptr = &vimpl_[0];
    std::string tmp;
    K_UINT_32 i;
    
    if (currentVectorContext->skipStartDelim_) {
      is >> tmp;
    }

    if (currentVectorContext->skipElemDelim_) {
      for (i = 0; i < n_-1; ++i) {
        is >> ptr[i] >> tmp;
      }
      is >> ptr[i];
    } else {
      for (i = 0; i < n_; ++i) {
        is >> ptr[i];
      }
    }

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

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

    const T* ptr = &vimpl_[0];
    K_UINT_32 i;

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

    os << currentVectorContext->startDelim_;
    for (i = 0; i < n_ - 1; ++i) {
      os.width(currentVectorContext->width_);
      os << ptr[i] << currentVectorContext->elemDelim_;
    }
    os.width(currentVectorContext->width_);
    os << ptr[i] << currentVectorContext->endDelim_;

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

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

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

}

#endif