hogman_minimal: hogman_minimal: axis

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 template <typename Base>
00018 _AxisAngle<Base>::_AxisAngle():_Vector<3, Base> (Base(0.),Base(0.),Base(0.)) {}
00019 
00020 template <typename Base>
00021 _AxisAngle<Base>::_AxisAngle(const _Quaternion<Base>& q) {
00022   Base angle=q.angle();
00023   Base imNorm = sqrt(q.x()*q.x()+q.y()*q.y()+q.z()*q.z());
00024   if (imNorm < std::numeric_limits<Base>::min()){
00025     this->x()=Base(0.);
00026     this->y()=Base(0.);
00027     this->z()=Base(0.);
00028   } else {
00029     Base alpha=angle/imNorm;
00030     this->x()=q.x()*alpha;
00031     this->y()=q.y()*alpha;
00032     this->z()=q.z()*alpha;
00033   }
00034 }
00035 
00036 template <typename Base>
00037 _AxisAngle<Base>::_AxisAngle(const _RotationMatrix3<Base>& m) {
00038   *this=_Quaternion<Base>(m);
00039 }
00040 
00041 template <typename Base>
00042 _AxisAngle<Base>::_AxisAngle(const _Vector<3, Base>& vec) {
00043   *this=_Quaternion<Base>(vec);
00044 }
00045 
00046 template <typename Base>
00047 _AxisAngle<Base>::_AxisAngle(Base roll, Base pitch, Base yaw){
00048   *this=_AxisAngle(_Quaternion<Base>(roll, pitch, yaw));
00049 }
00050 
00051 template <typename Base>
00052 _AxisAngle<Base>::_AxisAngle(const _Vector<3, Base>& axis, Base angle){
00053   *this = axis.normalized() * angle;
00054 }
00055 
00056 template <typename Base>
00057 _AxisAngle<Base>& _AxisAngle<Base>::operator*=(const _AxisAngle& a){
00058   *this=(*this)*a;
00059   return *this;
00060 }
00061   
00062   
00063 template <typename Base>
00064 _AxisAngle<Base>  _AxisAngle<Base>::operator* (const _AxisAngle& a) const{
00065   return _AxisAngle<Base>(quaternion()*a.quaternion());
00066 }
00067   
00068 template <typename Base>
00069 _Vector<3, Base> _AxisAngle<Base>::operator*(const _Vector<3, Base>& v) const {
00070   return rotationMatrix()*v;
00071 }
00072 
00073 template <typename Base>
00074 inline _AxisAngle<Base> _AxisAngle<Base>::inverse() const {
00075   _AxisAngle a(*this);
00076   a._Vector<3, Base>::operator*=(Base(-1.));
00077   return a;
00078 }
00079   
00080 template <typename Base>
00081 inline _Vector<3, Base> _AxisAngle<Base>::angles() const {
00082   return quaternion().angles();
00083 }
00084 
00085 template <typename Base>
00086 _RotationMatrix3<Base> _AxisAngle<Base>::rotationMatrix() const{
00087   return quaternion().rotationMatrix();
00088 }
00089 
00090 template <typename Base>
00091 _Quaternion<Base> _AxisAngle<Base>::quaternion() const {
00092   Base n=this->norm();
00093   if (n<=0){
00094     return _Quaternion<Base>();
00095   }
00096   Base s=sin(n*Base(.5))/n;
00097   Base c=cos(n*Base(.5));
00098   return _Quaternion<Base> (this->x()*s, this->y()*s, this->z()*s, c);
00099 }
00100
axis_angle.hpp
