Quaternion.hpp

Go to the documentation of this file.

// This file is part of GranOO, a workbench for DEM simulation.
//
// Author(s)    : - Damien Andre       IRCER/UNILIM, Limoges France
//                  <damien.andre@unilim.fr>
//                - Jean-luc Charles   Arts et Metiers ParisTech, CNRS, I2M, Bordeaux France
//                  <jean-luc.charles@ensam.eu>
//                - Jeremie Girardot   Arts et Metiers ParisTech, CNRS, I2M, Bordeaux France
//                  <jeremie.girardot@ensam.eu>
//                - Cedric Hubert      LAMIH/UPHF, Valenciennes France
//                  <cedric.hubert@uphf.fr>
//                - Ivan Iordanoff     Arts et Metiers ParisTech, CNRS, I2M, Bordeaux France
//                  <ivan.iordanoff@ensam.eu>
//
// Copyright (C) 2008-2019 D. Andre, JL. Charles, J. Girardot, C. Hubert, I. Iordanoff
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 
#ifndef _GranOO_libGeom_QUATERNION_hpp_
#define _GranOO_libGeom_QUATERNION_hpp_
 
#include <cmath>
#include <iostream>
#include "GranOO3/3rdParty/Eigen/Dense"
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
 
#include "GranOO3/Geom/Vector.hpp"
 
 
 
class TiXmlElement;
 
namespace GranOO3
{
  namespace Geom
  {
    class Tensor;
    class EulerAngle;
 
 
    class Quaternion
    {
 
      //Friend operator
      friend std::ostream& operator<< (std::ostream&  , const Quaternion&);
      friend Quaternion slerp      (const Quaternion& , const Quaternion&, const double& ratio);
      friend Quaternion operator*  (const Quaternion& , const Vector&     );
      friend Quaternion operator*  (const Vector &    , const Quaternion &);
      friend Quaternion operator*  (const Quaternion &, const Quaternion &);
      friend Quaternion operator*  (const Quaternion &, const double &    );
      friend Quaternion operator*  (const double &    , const Quaternion &);
      friend Quaternion operator+  (const Quaternion &, const Quaternion &);
      friend Quaternion operator-  (const Quaternion &, const Quaternion &);
      friend bool       operator== (const Quaternion &, const Quaternion &);
    public:
      // Usefull for internal use only, NOT DOCUMENTED !
      static Quaternion& glob(const std::string& id);
      static std::string class_ID() {return "Quaternion";}
      static Quaternion* new_object(const TiXmlElement* el);
      static const Quaternion& global;
      static const Quaternion& null;
      static const int N = 4;
 
    public:
      // Constructors and destructor
      Quaternion();
      Quaternion(const Vector& axis, double angle);
      explicit Quaternion(const Vector& v);
      Quaternion(double vx, double vy, double vz, double s);
      Quaternion(const Quaternion& q);
      Quaternion(const Quaternion& q, const Frame& from, const Frame& to);
      Quaternion(const Vector& from, const Vector& to);
      ~Quaternion();
 
      // Axis angle formalism
      void set_axis_angle(const Vector& axis, double angle);
      void set_angle(double angle);
      void set_vec_from_to(const Vector& from, const Vector& to);
      Vector get_axis() const;
      double get_angle() const;
      void get_axis_angle(Vector& axis, double& angle) const;
 
      // EulerAngle conversion
      void to_euler_angle(EulerAngle&) const;
      EulerAngle to_euler_angle() const;
      Quaternion & operator=(const EulerAngle &);
      
      // Accessors and mutators to components
      void set_value(double q0, double q1, double q2, double q3);
 
      // Component
      double & x();
      const double & x() const;
      double & y();
      const double & y() const;
      double & z();
      const double & z() const;
      double & r();
      const double & r() const;
 
 
      // Component (for python wrapper)
      void set_x(const double&);
      void set_y(const double&);
      void set_z(const double&);
      void set_r(const double&);
 
      // Component (for python wrapper)
      double get_x() const;
      double get_y() const;
      double get_z() const;
      double get_r() const;
 
 
      template <typename Axis> double& val();
      template <typename Axis> const double& val() const;
 
      const double& operator()(unsigned int i) const;
      double& operator()(unsigned int i);
 
 
      // Internal operators
      Quaternion& operator=(const Quaternion& Q);
      void operator*=(const Quaternion& Q);
      void operator*=(const Vector& v);
      void operator*=(const double& d);
      void operator+=(const Quaternion& Q);
      void operator-=(const Quaternion& Q);
 
      // rotate vectors
      Vector rotate(const Vector & v_in) const;
      Vector inverse_rotate(const Vector & v_in) const;
      void rotate(const Vector & v_in, Vector & v_out) const;
      void inverse_rotate(const Vector & v_in, Vector & v_out) const;
 
 
      // Miscellaneous usefull methods
      bool is_nan() const;
      void clear();
      void clear_unit();
      Quaternion conjugate() const;
      Tensor to_rotation_matrix() const;
      Vector to_vector() const;
 
      // Normalization
      double norm() const;
      double squared_norm() const;
      double normalize();
      Quaternion normalized() const;
      bool is_unit() const;
 
      // Misc
      std::string info() const;
      void add_glob(const std::string& id);
 
    private:
      void equalize(const Quaternion &);
      // - BOOST SERIALIZATION - //
      friend class boost::serialization::access;
      template<class Archive> void serialize(Archive & ar, const unsigned int );
 
    public:
      Eigen::Matrix<double, 4, 1> coord;
    };
 
#ifndef DOXYGEN_SHOULD_SKIP_THIS
 
    // - BOOST SERIALIZATION - //
 
    template<class Archive>
    void
    Quaternion::serialize(Archive & ar, const unsigned int ) {
      ar & x(); ar & y(); ar & z(); ar & coord(3);
    }
 
    //EXTERN OPERATOR
    std::ostream & operator<< (std::ostream& o, const Quaternion& q);
    Quaternion slerp      (const Quaternion& , const Quaternion&, const double& ratio);
    Quaternion operator*  (const Quaternion& , const Vector &    );
    Quaternion operator*  (const Vector &    , const Quaternion &);
    Quaternion operator*  (const Quaternion &, const Quaternion &);
    Quaternion operator*  (const Quaternion &, const double &    );
    Quaternion operator*  (const double &    , const Quaternion &);
    Quaternion operator+  (const Quaternion &, const Quaternion &);
    Quaternion operator-  (const Quaternion &, const Quaternion &);
    bool       operator== (const Quaternion &, const Quaternion &);
    
    inline
    Quaternion::Quaternion(const Vector &from, const Vector &to)
      : coord() {
      set_vec_from_to(from,to);
    }
 
    inline
    Quaternion::Quaternion()
      : coord() {
    }
 
    inline
    Quaternion::Quaternion(const Vector& axis, double angle)
      : coord() {
      set_axis_angle(axis, angle);
    }
 
    inline
    Quaternion::Quaternion(double q0, double q1, double q2, double real)
      : coord(q0, q1, q2, real){
    }
 
    inline
    Quaternion::Quaternion(const Quaternion& Q)
      :  coord(Q.coord) {
    }
 
    inline
    Quaternion::~Quaternion() {
    }
 
    inline double &
    Quaternion::x() {
      return coord(0);
    }
 
    inline const double &
    Quaternion::x() const {
      return coord(0);
    }
 
    inline double &
    Quaternion::y() {
      return coord(1);
    }
 
    inline const double &
    Quaternion::y() const {
      return coord(1);
    }
 
    inline double &
    Quaternion::z() {
      return coord(2);
    }
 
    inline const double &
    Quaternion::z() const {
      return coord(2);
    }
 
    inline double &
    Quaternion::r() {
      return coord(3);
    }
 
    inline const double &
    Quaternion::r() const {
      return coord(3);
    }
 
    inline void
    Quaternion::set_x(const double& val) {
      x() = val;
    }
 
    inline void
    Quaternion::set_y(const double& val) {
      y() = val;
    }
 
    inline void
    Quaternion::set_z(const double& val) {
      z() = val;
    }
 
    inline void
    Quaternion::set_r(const double& val) {
      coord(3) = val;
    }
 
    inline double
    Quaternion::get_x() const {
      return x();
    }
 
    inline double
    Quaternion::get_y() const {
      return y();
    }
 
    inline double
    Quaternion::get_z() const {
      return z();
    }
 
    inline double
    Quaternion::get_r() const {
      return coord(3);
    }
 
    template<typename Axis> inline
    double& Quaternion::val() {
      SafeModeAssert(0, "Non specilized method is forbidden");
      return coord(3);
    }
 
    template<typename Axis> inline
    const double& Quaternion::val() const {
      SafeModeAssert(0, "Non specilized method is forbidden");
      return coord(3);
    }
 
    template<> inline
    double& Quaternion::val<X>() {
      return x();
    }
 
    template<> inline
    const double& Quaternion::val<X>() const {
      return x();
    }
 
    template<> inline
    double& Quaternion::val<Y>() {
      return y();
    }
 
    template<> inline
    const double& Quaternion::val<Y>() const {
      return y();
    }
 
    template<> inline
    double& Quaternion::val<Z>() {
      return z();
    }
 
    template<> inline
    const double& Quaternion::val<Z>() const {
      return z();
    }
 
    template<> inline
    double& Quaternion::val<R>() {
      return coord(3);
    }
 
    template<> inline
    const double& Quaternion::val<R>() const {
      return coord(3);
    }
 
    inline const double&
    Quaternion::operator() (unsigned int i) const {
      SafeModeAssert(i < 4, "The component must be in [0, 3] range");
      return coord(i);
    }
 
    inline double&
    Quaternion::operator() (unsigned int i) {
      SafeModeAssert(i < 4, "The component must be in [0, 3] range");
      return coord(i);
    }
 
 
    inline void
    Quaternion::set_vec_from_to(const Vector& from, const Vector& to) {
      const float epsilon = 1E-10f;
 
      const float fromSqNorm = (float)(from.squared_norm());
      const float toSqNorm   = (float)(to.squared_norm());
      // Identity Quaternion when one vector is null
      if ((fromSqNorm < epsilon) || (toSqNorm < epsilon)) {
        x() = y() = z() = 0.;
        r() = 1.;
      } else {
        Vector axis = from^to;
        const float axisSqNorm = (float)(axis.squared_norm());
 
        // Aligned vectors, pick any axis, not aligned with from or to
        if (axisSqNorm < epsilon) {
          axis = from.orthogonal_vector();
        }
 
        double angle = asin(sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));
 
        if (from*to < 0.0) {
          angle = M_PI-angle;
        }
 
        set_axis_angle(axis, angle);
      }
 
    }
 
    inline void
    Quaternion::set_axis_angle(const Vector& axis, double angle) {
      const double norm = axis.norm();
      // if (norm < epsilon)
      //   {
      //     x = 0.0;
      //     y = 0.0;
      //     z = 0.0;
      //     r = 1.0;
      //    }
      // else
      // {
      const double sin_half_angle = sin(angle / 2.0);
      x() = sin_half_angle*axis.x()/norm;
      y() = sin_half_angle*axis.y()/norm;
      z() = sin_half_angle*axis.z()/norm;
      r() = cos(angle / 2.0);
      //}
    }
 
    inline void
    Quaternion::set_angle(double angle) {
      const Geom::Vector axis = get_axis();
      const double sin_half_angle = sin(angle / 2.0);
      x() = sin_half_angle*axis.x();
      y() = sin_half_angle*axis.y();
      z() = sin_half_angle*axis.z();
      r() = cos(angle / 2.0);
    }
 
    
  
    
    inline
    std::ostream&
    operator<<(std::ostream& o, const Quaternion& q) {
      return o << q.x() << '\t' << q.y() << '\t' << q.z() << '\t' << q.r();
    }
 
    inline
    Geom::Quaternion
    slerp(const Quaternion& q0, const Quaternion& q1, const double& ratio) {
      const double one = 1.0 - std::numeric_limits<double>::epsilon();
      double d = q0.x()*q1.x() + q0.y()*q1.y() + q0.z()*q1.z() + q0.r()*q1.r();
      double absD = abs(d);      
      double scale0;
      double scale1;
      if(absD >= one) {
    scale0 = 1.0 - ratio;
    scale1 = ratio;
      } else {
    double theta = acos(absD);
    double sinTheta = sin(theta);     
    scale0 = sin((1.0 - ratio) * theta) / sinTheta;
    scale1 = sin((ratio * theta)) / sinTheta;
      }
      
      if(d<0.0)
    scale1 = -scale1;  
      return Quaternion(scale0 * q0 + scale1 * q1);
    }
    
 
    inline
    Quaternion
    operator*(const Quaternion& q1, const Quaternion& q2) {
      return Quaternion (q1.r()*q2.x()  + q1.x()*q2.r()  + q1.y()*q2.z() - q1.z()*q2.y(),
                         q1.r()*q2.y()  + q1.y()*q2.r()  + q1.z()*q2.x() - q1.x()*q2.z(),
                         q1.r()*q2.z()  + q1.z()*q2.r()  + q1.x()*q2.y() - q1.y()*q2.x(),
                         q1.r()*q2.r()  - q1.x()* q2.x() - q1.y()*q2.y() - q1.z()*q2.z());
    }
 
    inline
    Quaternion
    operator*(const Quaternion& q, const Vector& v) {
      return Quaternion(q.r()*v.x() + q.y()*v.z() - q.z()*v.y(),
                        q.r()*v.y() + q.z()*v.x() - q.x()*v.z(),
                        q.r()*v.z() + q.x()*v.y() - q.y()*v.x(),
                        -v.x()*q.x() - q.y()*v.y() - q.z()*v.z() );
    }
 
    inline
    Quaternion
    operator*  (const Vector& v, const Quaternion& q) {
      return Quaternion(q.r()*v.x() + v.y()*q.z() - v.z()*q.y(),
                        q.r()*v.y() + v.z()*q.x() - v.x()*q.z(),
                        q.r()*v.z() + v.x()*q.y() - v.y()*q.x(),
                        -q.x()*v.x() - v.y()*q.y() - v.z()*q.z());
    }
 
    inline
    Quaternion
    operator*  (const Quaternion& q, const double& d) {
      return Quaternion(  q.x()*d, q.y()*d,  q.z()*d, q.r()*d );
    }
 
    inline
    Quaternion
    operator*  (const double& d, const Quaternion& q) {
      return Quaternion(  q.x()*d, q.y()*d,  q.z()*d, q.r()*d );
    }
 
    inline
    Quaternion
    operator+  (const Quaternion& q1, const Quaternion& q2) {
      return Quaternion(  q1.x()+q2.x(), q1.y()+q2.y(),  q1.z()+q2.z(), q1.r()+q2.r()  );
    }
 
    inline
    bool
    operator==  (const Quaternion& q1, const Quaternion& q2) {
      const double x = q1.x() - q2.x();
      const double y = q1.y() - q2.y();
      const double z = q1.z() - q2.z();
      const double r = q1.r() - q2.r();
      const double n = pow(x,2) + pow(y,2) + pow(z,2) + pow(r,2);
      return n < constant::epsilon;
    }
 
    
    inline
    Quaternion
    operator-  (const Quaternion& q1, const Quaternion& q2) {
      return Quaternion(  q1.x()-q2.x(), q1.y()-q2.y(),  q1.z()-q2.z(), q1.r()-q2.r()  );
    }
 
    inline
    void
    Quaternion::set_value(double q0, double q1, double q2, double q3) {
      x() = q0;
      y() = q1;
      z() = q2;
      r() = q3;
    }
 
    inline
    Quaternion& Quaternion::operator=(const Quaternion& Q) {
      x() = Q.x();
      y() = Q.y();
      z() = Q.z();
      r() = Q.r();
      return (*this);
    }
 
    inline
    void
    Quaternion::operator*=(const Vector &v) {
      const double q0(x());
      const double q1(y());
      const double q2(z());
      const double real(r());
      x() =   real*v.x() + q1*v.z() - q2*v.y();
      y() =   real*v.y() + q2*v.x() - q0*v.z();
      z() =   real*v.z() + q0*v.y() - q1*v.x();
      r () = -v.x()*q0   - q1*v.y() - q2*v.z();
    }
 
    inline
    void
    Quaternion::operator*=(const double &d) {
      x() *= d;
      y() *= d;
      z() *= d;
      r() *= d;
    }
 
    inline
    void
    Quaternion::operator*=(const Quaternion &q) {
      const double q0(x());
      const double q1(y());
      const double q2(z());
      const double real(r());
      x() = real*q.x() + q0*q.r() + q1*q.z() - q2*q.y();
      y() = real*q.y() + q1*q.r() + q2*q.x() - q0*q.z();
      z() = real*q.z() + q2*q.r() + q0*q.y() - q1*q.x();
      r() = real*q.r() - q0*q.x() - q1*q.y() - q2*q.z();
    }
 
    inline
    void
    Quaternion::operator+=(const Quaternion &Q) {
      x() += Q.x();
      y() += Q.y();
      z() += Q.z();
      r() += Q.r();
    }
 
    inline
    void
    Quaternion::operator-=(const Quaternion &Q) {
      x() -= Q.x();
      y() -= Q.y();
      z() -= Q.z();
      r() -= Q.r();
    }
 
    inline
    Quaternion
    Quaternion::conjugate() const {
      return Quaternion (-x(), -y(), -z(), r());
    }
 
    inline
    double
    Quaternion::squared_norm() const {
      return (x()*x() + y()*y() + z()*z() + r()*r());
    }
 
    inline
    double
    Quaternion::norm() const {
      return sqrt(x()*x() + y()*y() + z()*z() + r()*r());
    }
 
    inline
    double
    Quaternion::normalize() {
      const double norm = sqrt(x()*x() + y()*y() + z()*z() + r()*r());
      x() /= norm;
      y() /= norm;
      z() /= norm;
      r() /= norm;
      return norm;
    }
 
    inline
    Quaternion
    Quaternion::normalized() const {
      double Q[4];
      const double norm = sqrt(x()*x() + y()*y() + z()*z() + r()*r());
      Q[0] = x() / norm;
      Q[1] = y() / norm;
      Q[2] = z() / norm;
      Q[3] = r() / norm;
      return Quaternion(Q[0], Q[1], Q[2], Q[3]);
    }
 
    inline
    bool
    Quaternion::is_unit() const {
      return (fabs(x()*x() + y()*y() + z()*z()+ r()*r())-1.0f) < 1.0e-8f;
    }
 
    inline
    void
    Quaternion::rotate(const Vector& v_in, Vector& v_out) const {
      const double q00 = 2.0 * x() * x();
      const double q11 = 2.0 * y() * y();
      const double q22 = 2.0 * z() * z();
      const double q01 = 2.0 * x() * y();
      const double q02 = 2.0 * x() * z();
      const double q03 = 2.0 * x() * r();
      const double q12 = 2.0 * y() * z();
      const double q13 = 2.0 * y() * r();
      const double q23 = 2.0 * z() * r();
 
      v_out.x() = (1.0 - q11 - q22) * (v_in.x()) + (q01 - q23) * v_in.y() + (q02 + q13) * v_in.z();
      v_out.y() = (q01 + q23) * v_in.x() + (1.0 - q22 - q00) * v_in.y() + (q12 - q03) * v_in.z();
      v_out.z() = (q02 - q13) * v_in.x() + (q12 + q03) * v_in.y() + (1.0 - q11 - q00) * v_in.z();
    }
 
    inline
    void
    Quaternion::inverse_rotate(const Vector & v_in, Vector & v_out) const {
      const double q00 =   2.0 * x() * x();
      const double q11 =   2.0 * y() * y();
      const double q22 =   2.0 * z() * z();
      const double q01 =   2.0 * x() * y();
      const double q02 =   2.0 * x() * z();
      const double q03 = -2.0 * x() * r();
      const double q12 =   2.0 * y() * z();
      const double q13 = -2.0 * y() * r();
      const double q23 = -2.0 * z() * r();
 
      v_out.x() = (1.0 - q11 - q22)*v_in.x() + (q01 - q23)*v_in.y() + (q02 + q13)*v_in.z();
      v_out.y() = (q01 + q23)*v_in.x() + (1.0 - q22 - q00)*v_in.y() + (q12 - q03)*v_in.z();
      v_out.z() = (q02 - q13)*v_in.x() + (q12 + q03)*v_in.y() + (1.0 - q11 - q00)*v_in.z();
    }
 
    inline
    Vector
    Quaternion::rotate(const Vector & v_in) const {
      const double q00 = 2.0 * x() * x();
      const double q11 = 2.0 * y() * y();
      const double q22 = 2.0 * z() * z();
      const double q01 = 2.0 * x() * y();
      const double q02 = 2.0 * x() * z();
      const double q03 = 2.0 * x() * r();
      const double q12 = 2.0 * y() * z();
      const double q13 = 2.0 * y() * r();
      const double q23 = 2.0 * z() * r();
 
      return Vector ((1.0 - q11 - q22)*v_in.x() + (q01 - q23)*v_in.y() + (q02 + q13)*v_in.z(),
                     (q01 + q23)*v_in.x() + (1.0 - q22 - q00)*v_in.y() + (q12 - q03)*v_in.z(),
                     (q02 - q13)*v_in.x() + (q12 + q03)*v_in.y() + (1.0 - q11 - q00)*v_in.z());
 
    }
 
    inline
    Vector
    Quaternion::inverse_rotate(const Vector & v_in) const {
      const double q00 =   2.0 * x() * x();
      const double q11 =   2.0 * y() * y();
      const double q22 =   2.0 * z() * z();
      const double q01 =   2.0 * x() * y();
      const double q02 =   2.0 * x() * z();
      const double q03 = -2.0 * x() * r();
      const double q12 =   2.0 * y() * z();
      const double q13 = -2.0 * y() * r();
      const double q23 = -2.0 * z() * r();
 
      return Vector ((1.0 - q11 - q22)*v_in.x() + (q01 - q23)*v_in.y() + (q02 + q13)*v_in.z(),
                     (q01 + q23)*v_in.x() + (1.0 - q22 - q00)*v_in.y() + (q12 - q03)*v_in.z(),
                     (q02 - q13)*v_in.x() + (q12 + q03)*v_in.y() + (1.0 - q11 - q00)*v_in.z());
    }
 
    inline bool
    Quaternion::is_nan() const {
      if (x()!=x())
        return true;
      if (y()!=y())
        return true;
      if (z()!=z())
        return true;
      if (r()!=r())
        return true;
      return false;
    }
 
    inline
    void
    Quaternion::clear() {
      x() = 0.;
      y() = 0.;
      z() = 0.;
      r() = 0.;
    }
 
    inline
    void
    Quaternion::clear_unit() {
      x() = 0.;
      y() = 0.;
      z() = 0.;
      r() = 1.;
    }
 
    inline
    Vector
    Quaternion::to_vector() const {
      return Vector(x(),y(),z());
    }
 
    inline
    void
    Quaternion::equalize(const Quaternion &Q) {
      x() = Q.x();
      y() = Q.y();
      z() = Q.z();
      r() = Q.r();
    }
 
    inline
    Quaternion::Quaternion(const Vector& v)
      : coord(v.x(), v.y(), v.z(), 0.) {
    }
 
 
#endif
  }
}
 
#endif