vector_n.h

Go to the documentation of this file.

// HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
// Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
// 
// HOG-Man 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.
// 
// HOG-Man 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 program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef _VECTORN_H_
#define _VECTORN_H_
#include <assert.h>
#include <iostream>
#include <stuff/array_allocator.h>


template <int Rows, int Cols, typename Base>
  struct _Matrix;

template <int N, typename Base=double>
struct _Vector{
  typedef Base BaseType;

  inline int size() const {return _allocator.size();}

  inline const Base& operator[](int i) const {return _allocator[i];}
  inline bool operator==(const _Vector<N, Base>& other) const;

  inline Base& operator[](int i) {return _allocator[i];}

  _Vector(int s=N): _allocator(s){}
  static const int TemplateSize=N;

  template <typename Base2>
    _Vector(const _Vector<N, Base2>& v);
  
  _Vector(Base v0, Base v1){_allocator[0]=v0; _allocator[1]=v1;}

  _Vector(Base v0, Base v1, Base v2) {_allocator[0]=v0; _allocator[1]=v1; _allocator[2]=v2;}

  _Vector(Base v0, Base v1, Base v2, Base v3) {_allocator[0]=v0; _allocator[1]=v1; _allocator[2]=v2; _allocator[3]=v3;}


  // convenience accessor functions for transformations
  inline const Base& x()     const  {return _allocator[0];}

  inline const Base& y()     const  {return _allocator[1];}

  inline const Base& z()     const  {return _allocator[2];}

  inline const Base& w()     const  {return _allocator[3];}

  inline Base& x()            {return _allocator[0];}

  inline Base& y()            {return _allocator[1];}

  inline Base& z()            {return _allocator[2];}

  inline Base& w()            {return _allocator[3];}

  // rotational view
  inline const Base& roll()   const  {if (size() == 6) return _allocator[3]; return _allocator[0];}

  inline const Base& pitch() const  {if (size() == 6) return _allocator[4]; return _allocator[1];}

  inline const Base& yaw()   const  {if (size() == 6) return _allocator[5]; return _allocator[2];}

  inline Base& roll()          {if (size() == 6) return _allocator[3]; return _allocator[0];}

  inline Base& pitch()        {if (size() == 6) return
 _allocator[4]; return _allocator[1];}

  inline Base& yaw()          {if (size() == 6) return _allocator[5]; return _allocator[2];}


  _Vector<N,Base> operator + (const _Vector<N,Base>& v) const;

  _Vector<N,Base>& operator += (const _Vector<N,Base>& v);

  _Vector<N,Base> operator - (const _Vector<N,Base>& v) const;

  _Vector<N,Base>& operator -= (const _Vector<N,Base>& v);

  Base operator *(const _Vector<N,Base>& v) const;

  _Vector<N,Base>& operator *= (Base c);

  _Vector<N,Base> operator * (Base c) const;

  Base squaredNorm() const;

  Base norm() const;

  void normalize();

  void fill(Base scalar);

  _Vector<N,Base> normalized() const;

  operator _Matrix<N, 1, Base>() const;

  _ArrayAllocator<N,Base> _allocator;

};

template <int N, typename Base>
  std::ostream& operator << (std::ostream& os, const _Vector<N, Base>& v);

template <int N, typename Base>
_Vector<N, Base> operator* (Base x, const _Vector<N, Base>& v);

typedef _Vector<2,int>   Vector2i;
typedef _Vector<2,float> Vector2f;
typedef _Vector<3,float> Vector3f;
typedef _Vector<6,float> Vector6f;


typedef _Vector<2,double> Vector2;
typedef _Vector<3,double> Vector3;
typedef _Vector<6,double> Vector6;

// dynamic size vectors
typedef _Vector<0,double> VectorX;
typedef _Vector<0,float>  VectorXf;

template <int N, typename Base>
  void st2dyn(_Vector<0,Base>& dest, const _Vector<N,Base> src){
  std2dyn(dest._allocator, src._allocator);
}

template <int N, typename Base>
  void dyn2st(_Vector<0,Base>& dest, const _Vector<N,Base> src){
  dyn2st(dest._allocator, src._alloator);
}


#include "vector_n.hpp"

#endif