vector_n.hpp
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 // 
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // HOG-Man is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #include <cmath>
00018 #include <algorithm>
00019 
00020 template <int N, typename Base>
00021 template <typename Base2>
00022 _Vector<N,Base>::_Vector(const _Vector<N, Base2>& v){
00023   for (int i=0; i<size(); i++)
00024     _allocator[i]=Base(0.);
00025   int s=std::min(size(),v.size());
00026   for (int i=0; i<s; i++)
00027     _allocator[i]=v._allocator[i];
00028 }
00029 
00030 template <int N, typename Base>
00031 _Vector<N,Base> _Vector<N,Base>::operator + (const _Vector<N,Base>& v) const {
00032   _Vector<N,Base> ret(*this);
00033   ret+=v;
00034   return ret;
00035 };
00036 
00037 template <int N, typename Base>
00038 _Vector<N,Base>& _Vector<N,Base>::operator += (const _Vector<N,Base>& v) {
00039   for (int i=0; i<size(); i++)
00040     _allocator[i]+=v._allocator[i];
00041   return *this;
00042 };
00043 
00044 template <int N, typename Base>
00045 _Vector<N,Base> _Vector<N,Base>::operator - (const _Vector<N,Base>& v) const {
00046   _Vector<N,Base> ret(*this);
00047   ret-=v;
00048   return ret;
00049 };
00050 
00051 template <int N, typename Base>
00052 _Vector<N,Base>& _Vector<N,Base>::operator -= (const _Vector<N,Base>& v) {
00053   for (int i=0; i<size(); i++)
00054     _allocator[i]-=v._allocator[i];
00055   return *this;
00056 };
00057 
00058 template <int N, typename Base>
00059 Base _Vector<N,Base>::operator *(const _Vector<N,Base>& v) const{
00060   Base acc=Base(0);
00061   for (int i=0; i<size(); i++)
00062     acc+=_allocator[i]*v._allocator[i];
00063   return acc;
00064 }
00065 
00066 template <int N, typename Base>
00067 _Vector<N,Base>& _Vector<N,Base>::operator *= (Base c){
00068   for (int i=0; i<size(); i++)
00069     _allocator[i]*=c;
00070   return *this;
00071 }
00072 
00073 template <int N, typename Base>
00074 _Vector<N,Base> _Vector<N,Base>::operator * (Base c) const{
00075   _Vector<N,Base> ret(*this);
00076   ret*=c;
00077   return ret;
00078 }
00079 
00080 template <int N, typename Base>
00081 bool _Vector<N,Base>::operator==(const _Vector<N, Base>& other) const {
00082   for (int i=0; i<size(); ++i)
00083     if ((*this)[i]!=other[i]) return false;
00084   return true;
00085 }
00086 
00087 template <int N, typename Base>
00088 Base _Vector<N,Base>::squaredNorm() const{
00089   Base acc=Base(0);
00090   for (int i=0; i<size(); i++)
00091     acc+=_allocator[i]*_allocator[i];
00092   return acc;
00093 }
00094 
00095 template <int N, typename Base>
00096 Base _Vector<N,Base>::norm() const{
00097   return sqrt(squaredNorm());
00098 }
00099 
00100 
00101 template <int N, typename Base>
00102 void _Vector<N, Base>::normalize(){
00103   Base f=squaredNorm();
00104   if (f>Base(0))
00105     (*this)*=sqrt(Base(1.)/f);
00106   else {
00107     (*this)-=(*this);
00108     _allocator[0]=Base(1.);
00109   }
00110 }
00111 
00112 template <int N, typename Base>
00113 _Vector<N,Base> _Vector<N, Base>::normalized() const{
00114   _Vector<N,Base> aux(*this);
00115   aux.normalize();
00116   return aux;
00117 }
00118 
00119 template <int N, typename Base>
00120 std::ostream& operator << (std::ostream& os, const _Vector<N, Base>& v){
00121   for (int j=0; j<v.size(); j++){
00122     if (j > 0)
00123       os << " ";
00124     os << v[j];
00125   }
00126   return os;
00127 }
00128 
00129 template <int N, typename Base>
00130 void _Vector<N, Base>::fill(Base scalar)
00131 {
00132   for (int i=0; i<size(); i++)
00133     _allocator[i] = scalar;
00134 }
00135 
00136 template <int N, typename Base>
00137 _Vector<N, Base> operator* (Base x, const _Vector<N, Base>& v)
00138 {
00139   return v * x;
00140 }
00141 


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:59