Quaternion.hh

/*
 * Copyright 2011 Nate Koenig & Andrew Howard
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/* Desc: External interfaces for Gazebo
 * Author: Nate Koenig
 * Date: 03 Apr 2007
 * SVN: $Id$
 */

#ifndef QUATERN_HH
#define QUATERN_HH

#include <iostream>
#include <math.h> 
#include <cmath> 

#include "math/Angle.hh"
#include "math/Vector3.hh"
#include "math/Matrix3.hh"
#include "math/Matrix4.hh"

namespace gazebo
{
    namespace math
  {
  
  
  class Quaternion
  {
    public: Quaternion();
  
    public: Quaternion( const double &w, const double &x, const double &y, 
                        const double &z);

    public: Quaternion( const double &_roll, const double &_pitch, 
                        const double &_yaw );

    public: Quaternion( const Vector3 &_axis, const double &_angle );

    public: Quaternion( const Vector3 &_rpy);
 
    public: Quaternion( const Quaternion &qt );
  
    public: ~Quaternion();
  
    public: const Quaternion &operator=(const Quaternion &qt);
  
    public: void Invert();
  
    public: inline Quaternion GetInverse() const
            {
              Quaternion q;
              double norm = this->w*this->w + this->x*this->x +
                            this->y*this->y + this->z*this->z;

              if (norm > 0.0)
              {
                q.w = this->w / norm;
                q.x = -this->x / norm;
                q.y = -this->y / norm;
                q.z = -this->z / norm;
              }
              return q;
            }
  
    public: void SetToIdentity();

    public: Quaternion GetLog() const;
    public: Quaternion GetExp() const;
  
    public: void Normalize();
  
    public: void SetFromAxis(double x, double y, double z, double a);

    public: void SetFromAxis(const Vector3 &_axis, double _a);
  
    public: void Set(double u, double x, double y, double z);
           
    public: void SetFromEuler(const Vector3 &vec);
  
    public: Vector3 GetAsEuler() const;
  
    public: static Quaternion EulerToQuaternion( const Vector3 &vec );
  
    public: static Quaternion EulerToQuaternion( double x, double y, double z);
  
    public: double GetRoll();
  
    public: double GetPitch();
  
    public: double GetYaw();
  
    public: void GetAsAxis(Vector3 &axis, double &angle) const;
  
    public: void Scale(double scale);
  
    public: Quaternion operator+( const Quaternion &qt ) const;
  
    public: Quaternion operator+=( const Quaternion &qt );
  
    public: Quaternion operator-( const Quaternion &qt ) const;
  
    public: Quaternion operator-=( const Quaternion &qt );
  
    public: inline Quaternion operator*( const Quaternion &_q ) const
            {
              return Quaternion(
                  this->w*_q.w - this->x*_q.x - this->y*_q.y - this->z*_q.z,
                  this->w*_q.x + this->x*_q.w + this->y*_q.z - this->z*_q.y,
                  this->w*_q.y - this->x*_q.z + this->y*_q.w + this->z*_q.x,
                  this->w*_q.z + this->x*_q.y - this->y*_q.x + this->z*_q.w);
            }

    public: Quaternion operator*( const double &_f ) const;
  
    public: Quaternion operator*=( const Quaternion &qt );
  
    public: Vector3 operator*( const Vector3 &v ) const;
  
    public: bool operator==(const Quaternion &_qt) const;

    public: bool operator!=(const Quaternion &_qt) const;

    public: Quaternion operator-() const;

    public: inline Vector3 RotateVector(const Vector3 &_vec) const
            {
              Quaternion tmp(0.0, _vec.x, _vec.y, _vec.z);
              tmp = (*this) * (tmp * this->GetInverse());
              return Vector3(tmp.x, tmp.y, tmp.z);
            }
  
    public: Vector3 RotateVectorReverse(Vector3 vec) const;
  
    public: bool IsFinite() const;
  
    public: inline void Correct()
            {
              if (!finite(this->x))
                this->x = 0;
              if (!finite(this->y))
                this->y = 0;
              if (!finite(this->z))
                this->z = 0;
              if (!finite(this->w))
                this->w = 1;

              if (this->w == 0 && this->x == 0 && this->y == 0 && this->z == 0)
                this->w = 1;
            }

    public: Matrix3 GetAsMatrix3() const;

    public: Matrix4 GetAsMatrix4() const;
  
    public: Vector3 GetXAxis() const;
    public: Vector3 GetYAxis() const;
    public: Vector3 GetZAxis() const;
  
    public: void Round(int _precision);

    public: double Dot(const Quaternion &_q) const;

    public: static Quaternion Squad(double _fT, const Quaternion &_rkP,
                const Quaternion &_rkA, const Quaternion &_rkB,
                Quaternion &_rkQ, bool _shortestPath = false);

    public: static Quaternion Slerp (double _fT, const Quaternion &_rkP,
                const Quaternion &_rkQ, bool _shortestPath = false);
                  

    public: double w;
  
    public: double x;
  
    public: double y;
  
    public: double z;
  
    public: friend  std::ostream &operator<<( std::ostream &out, 
                const gazebo::math::Quaternion &q )
    {
      Vector3 v( q.GetAsEuler() );
      //v.x = v.x * 180.0 / M_PI;
      //v.y = v.y * 180.0 / M_PI;
      //v.z = v.z * 180.0 / M_PI;
  
      /*if (std::isnan(v.x))
        v.x = 90.0;
      if (std::isnan(v.y))
        v.y = 90.0;
      if (std::isnan(v.z))
        v.z = 90.0;
        */
  
      out << v.x << " " << v.y << " " << v.z;
  
      return out;
    }
  
    public: friend std::istream &operator>>( std::istream &in, 
                                             gazebo::math::Quaternion &q )
    {
      Angle r, p, y;
  
      // Skip white spaces
      in.setf( std::ios_base::skipws );
      in >> r >> p >> y;

      q.SetFromEuler(Vector3(*r,*p,*y));
  
      return in;
    }
  
  
  };
  
  }

}
#endif