quaternion.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 <limits>
00018 #include <algorithm>
00019 
00020 template<typename Base>
00021 _Quaternion<Base>::_Quaternion(): _Vector<4, Base>(Base(0), Base(0), Base(0), Base(1)){
00022 }
00023 
00024 template<typename Base>
00025 _Quaternion<Base>::_Quaternion(const _Vector<4, Base>& v): _Vector<4, Base>(v){
00026 }
00027 
00028 template<typename Base>
00029 _Quaternion<Base>::_Quaternion(Base _x, Base _y, Base _z, Base _w): _Vector<4, Base>(_x,_y,_z,_w){
00030 }
00031 
00032 template<typename Base>
00033 _Quaternion<Base>::_Quaternion(const _RotationMatrix3<Base>& _r){
00034   this->x()=_r[2][1]-_r[1][2];
00035   this->y()=_r[0][2]-_r[2][0];
00036   this->z()=_r[1][0]-_r[0][1];
00037   this->w()=Base(1.)+_r[0][0]+_r[1][1]+_r[2][2];
00038   this->normalize();
00039 }
00040 
00041 template<typename Base>
00042 _Quaternion<Base>::_Quaternion(const _Vector<3, Base>& vec){
00043   *this=_Quaternion(_RotationMatrix3<Base>(vec));
00044 }
00045 
00046 template<typename Base>
00047 _Quaternion<Base>::_Quaternion(Base roll, Base pitch, Base yaw){
00048   *this=_Quaternion(_RotationMatrix3<Base>(roll, pitch, yaw));
00049 }
00050  
00051 template<typename Base>
00052 inline _Quaternion<Base> _Quaternion<Base>::inverse() const{
00053   return _Quaternion<Base>(-this->x(), -this->y(), -this->z(), this->w());
00054 }
00055 
00056 template<typename Base>
00057 inline _Quaternion<Base>& _Quaternion<Base>::normalize(){
00058   Base n = this->w()>0 ? this->norm() : -this->norm();
00059   if (fabs(n) > 1e-9){
00060     this->_Vector<4, Base>::operator*=(Base(1.)/n);
00061   }
00062   else
00063     *this= _Quaternion<Base>(0.,0.,0.,1.);
00064   return *this;
00065 }
00066 
00067 template<typename Base>
00068 inline _Quaternion<Base> _Quaternion<Base>::normalized() const{
00069   _Quaternion<Base> q(*this);
00070   return q.normalize();
00071 }
00072 
00073 
00074 template<typename Base>
00075 inline _Vector<3, Base> _Quaternion<Base>::operator * (const _Vector<3, Base>& point) const{
00076   const double sa =   point[0]*this->x() + point[1]*this->y() + point[2]*this->z();
00077   const double sb =   point[0]*this->w() - point[1]*this->z() + point[2]*this->y();
00078   const double sc =   point[0]*this->z() + point[1]*this->w() - point[2]*this->x();
00079   const double sd = - point[0]*this->y() + point[1]*this->x() + point[2]*this->w();
00080   _Vector<3, Base> out;
00081   out[0] = this->w() * sb + this->x() * sa + this->y() * sd - this->z() * sc;
00082   out[1] = this->w() * sc - this->x() * sd + this->y() * sa + this->z() * sb;
00083   out[2] = this->w() * sd + this->x() * sc - this->y() * sb + this->z() * sa;
00084   return out;
00085 }
00086 
00087 template<typename Base>
00088 inline _Quaternion<Base> _Quaternion<Base>::operator * (const _Quaternion<Base>& q2) const{
00089   _Quaternion<Base> q(this->y()*q2.z() - q2.y()*this->z() + this->w()*q2.x() + q2.w()*this->x(),
00090                      this->z()*q2.x() - q2.z()*this->x() + this->w()*q2.y() + q2.w()*this->y(),
00091                      this->x()*q2.y() - q2.x()*this->y() + this->w()*q2.z() + q2.w()*this->z(),
00092                      this->w()*q2.w() - this->x()*q2.x() - this->y()*q2.y() - this->z()*q2.z());
00093   //  return q.normalize();
00094   return q.normalize();
00095   //return Quaternion(q.rotationMatrix());
00096 }
00097 
00098 template<typename Base>
00099 inline _Quaternion<Base>& _Quaternion<Base>::operator *= (const _Quaternion<Base>& q2){
00100   _Quaternion<Base> q=(*this*q2);
00101   *this=q.normalize();
00102   return *this;
00103 }
00104 
00105 
00106 template<typename Base>
00107 inline _Vector<3, Base> _Quaternion<Base>::angles() const
00108 {
00109   return rotationMatrix().angles();
00110 }
00111 
00112 template<typename Base>
00113 _RotationMatrix3<Base> _Quaternion<Base>::rotationMatrix() const 
00114 {
00115   _RotationMatrix3<Base> rot;
00116   Base w2=this->w()*this->w();
00117   Base x2=this->x()*this->x();
00118   Base y2=this->y()*this->y();
00119   Base z2=this->z()*this->z();
00120   Base xy=this->x()*this->y();
00121   Base xz=this->x()*this->z();
00122   Base xw=this->x()*this->w();
00123   Base yz=this->y()*this->z();
00124   Base yw=this->y()*this->w();
00125   Base zw=this->z()*this->w();
00126   rot[0][0] = w2 + x2 - y2 - z2; rot[0][1] = 2.0 * (xy - zw);   rot[0][2] = 2.0 * (xz + yw);
00127   rot[1][0] = 2.0 * (xy + zw);   rot[1][1] = w2 - x2 + y2 - z2; rot[1][2] = 2.0 * (yz - xw);
00128   rot[2][0] = 2.0 * (xz - yw);   rot[2][1] = 2.0 * (yz + xw);   rot[2][2] = w2 - x2 - y2 + z2;
00129   return rot;
00130 }
00131 
00132 template<typename Base>
00133 inline Base _Quaternion<Base>::angle() const{
00134   _Quaternion<Base> q=normalized();
00135   double a=2*atan2(sqrt(q.x()*q.x() + q.y()*q.y()  + q.z()*q.z()), q.w());
00136   return atan2(sin(a), cos(a));
00137 }
00138 
00139 template<typename Base>
00140 inline _Quaternion<Base> _Quaternion<Base>::slerp(const _Quaternion<Base>& from, const _Quaternion<Base>& to, Base lambda){
00141   Base _cos_omega = _Vector<4, Base>(from)*_Vector<4, Base>(to);
00142         _cos_omega = (_cos_omega>1)?1:_cos_omega;
00143         _cos_omega = (_cos_omega<-1)?-1:_cos_omega;
00144         Base _omega = acos(_cos_omega);
00145         assert (!isnan(_cos_omega));
00146         if (fabs(_omega) < 1e-9)
00147                 return to;
00148         
00149         //determine right direction of slerp:
00150         _Quaternion<Base> _pq = from - to;
00151         _Quaternion<Base> _pmq = from + to;
00152         Base _first = _pq.norm();
00153         Base _alternativ = _pmq.norm();
00154         
00155         _Quaternion<Base> q1 = from;
00156         _Quaternion<Base> q2 = to;
00157 
00158         if (_first > _alternativ)
00159           q2._Vector<4, Base>::operator*=(Base(-1.));
00160 
00161         //now calculate intermediate quaternion.
00162         Base alpha=sin((1-lambda)*_omega)/(sin(_omega));
00163         Base beta=sin((lambda)*_omega)/(sin(_omega));
00164         _Quaternion<Base> ret((_Vector<4, Base>)q1*(alpha) + (_Vector<4, Base>)q2*(beta));
00165         assert (!(isnan(ret.w()) || isnan(ret.x()) || isnan(ret.y()) || isnan(ret.z())));
00166         return ret.normalized();
00167 }
00168 
00169 template<typename Base>
00170 template<typename Base2>
00171 _Quaternion<Base>::_Quaternion(const _Quaternion<Base2>& other){
00172   (*this)[0] = other[0];
00173   (*this)[1] = other[1];
00174   (*this)[2] = other[2];
00175   (*this)[3] = other[3];
00176 }


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