Quaternion.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2016 Open Source Robotics Foundation
 *
 * 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.
 *
*/

#ifndef _GAZEBO_MATH_QUATERNION_HH_
#define _GAZEBO_MATH_QUATERNION_HH_

#include <math.h>
#include <iostream>
#include <cmath>
#include <ignition/math/Quaternion.hh>

#include "gazebo/math/Helpers.hh"
#include "gazebo/math/Angle.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Matrix3.hh"
#include "gazebo/math/Matrix4.hh"
#include "gazebo/util/system.hh"

namespace gazebo
{
  namespace math
  {

  class GZ_MATH_VISIBLE 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(const ignition::math::Quaterniond &_qt);

    public: ~Quaternion();

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

    public: ignition::math::Quaterniond Ign() const;

    public: Quaternion &operator =(const ignition::math::Quaterniond &_v);

    public: void Invert();

    public: inline Quaternion GetInverse() const
            {
              double s = 0;
              Quaternion q(this->w, this->x, this->y, this->z);

              // use s to test if quaternion is valid
              s = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;

              if (math::equal(s, 0.0))
              {
                q.w = 1.0;
                q.x = 0.0;
                q.y = 0.0;
                q.z = 0.0;
              }
              else
              {
                // deal with non-normalized quaternion
                // div by s so q * qinv = identity
                q.w =  q.w / s;
                q.x = -q.x / s;
                q.y = -q.y / s;
                q.z = -q.z / s;
              }
              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: void SetFromEuler(double _roll, double _pitch, double _yaw);

    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 (!std::isfinite(this->x))
                this->x = 0;
              if (!std::isfinite(this->y))
                this->y = 0;
              if (!std::isfinite(this->z))
                this->z = 0;
              if (!std::isfinite(this->w))
                this->w = 1;

              if (math::equal(this->w, 0.0) &&
                  math::equal(this->x, 0.0) &&
                  math::equal(this->y, 0.0) &&
                  math::equal(this->z, 0.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,
                const Quaternion &_rkQ, bool _shortestPath = false);

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

    public: Quaternion Integrate(const Vector3 &_angularVelocity,
                                 const double _deltaT) const;

    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());
      _out << precision(v.x, 6) << " " << precision(v.y, 6) << " "
           << precision(v.z, 6);
      return _out;
    }

    public: friend std::istream &operator>>(std::istream &_in,
                                             gazebo::math::Quaternion &_q)
    {
      Angle roll, pitch, yaw;

      // Skip white spaces
      _in.setf(std::ios_base::skipws);
      _in >> roll >> pitch >> yaw;

      _q.SetFromEuler(Vector3(*roll, *pitch, *yaw));

      return _in;
    }
  };
  }
}
#endif