JQuaternion3D.hh

Go to the documentation of this file.

#ifndef __JQUATERNION3D__
#define __JQUATERNION3D__

#include <istream>
#include <ostream>
#include <limits>
#include <cmath>

#include "JIO/JSerialisable.hh"
#include "JLang/JEquals.hh"
#include "JMath/JMath.hh"
#include "JGeometry3D/JVector3D.hh"
#include "JGeometry3D/JVersor3D.hh"


/**
 * \author mdejong
 */

namespace JGEOMETRY3D {}
namespace JPP { using namespace JGEOMETRY3D; }

namespace JGEOMETRY3D {

  using JIO::JReader;
  using JIO::JWriter;
  using JMATH::JMath;
  using JLANG::JEquals;
  
  
  /**
   * This class represents a rotation.
   */
  struct JQuaternion2D {
  protected:
    /**
     * Default constructor.
     * This constructor corresponds to the identity operation.
     */
    JQuaternion2D() :
      __a(1.0),
      __u(0.0)
    {}


    /**
     * Constructor.
     *
     * \param  theta         rotation angle [rad]
     */
    JQuaternion2D(const double theta) :
      __a(cos(0.5*theta)),
      __u(sin(0.5*theta))
    {}


  public:
    /**
     * Get a value.
     *
     * \return               a value
     */
    double getA() const 
    { 
      return __a;
    }


    /**
     * Write quaternion from input.
     *
     * \param  in            input stream
     * \param  quaternion    quaternion
     * \return               input stream
     */
    friend inline std::istream& operator>>(std::istream& in, JQuaternion2D& quaternion)
    {
      return in >> quaternion.__a >> quaternion.__u;
    }


    /**
     * Write quaternion to output.
     *
     * \param  out           output stream
     * \param  quaternion    quaternion
     * \return               output stream
     */
    friend inline std::ostream& operator<<(std::ostream& out, const JQuaternion2D& quaternion)
    {
      return out << quaternion.__a << ' ' << quaternion.__u;
    }
    

    /**
     * Read quaternion from input.
     *
     * \param  in            reader
     * \param  quaternion    quaternion
     * \return               reader
     */
    friend inline JReader& operator>>(JReader& in, JQuaternion2D& quaternion)
    {
      return in >> quaternion.__a >> quaternion.__u;
    }


    /**
     * Write quaternion to output.
     *
     * \param  out           writer
     * \param  quaternion    quaternion
     * \return               writer
     */
    friend inline JWriter& operator<<(JWriter& out, const JQuaternion2D& quaternion)
    {
      return out << quaternion.__a << quaternion.__u;
    }


  protected:
    double __a;
    double __u;
  };

  
  /**
   * This class represents a rotation around the x-axis.
   */
  struct JQuaternion3X :
    public JQuaternion2D
  {
    /**
     * Default constructor.
     */
    JQuaternion3X() :
      JQuaternion2D()
    {}

    
    /**
     * Constructor.
     *
     * \param  theta         rotation angle [rad]
     */
    JQuaternion3X(const double theta) :
      JQuaternion2D(theta)
    {}


    /**
     * Get b value.
     *
     * \return               b value
     */
    double getB() const 
    { 
      return __u;
    }
  };

  
  /**
   * This class represents a rotation around the y-axis.
   */
  struct JQuaternion3Y  :
    public JQuaternion2D
  {
    /**
     * Default constructor.
     */
    JQuaternion3Y() :
      JQuaternion2D()
    {}

    
    /**
     * Constructor.
     *
     * \param  theta         rotation angle [rad]
     */
    JQuaternion3Y(const double theta) :
      JQuaternion2D(theta)
    {}

    
    /**
     * Get c value.
     *
     * \return               c value
     */
    double getC() const 
    { 
      return __u;
    }
  };

  
  /**
   * This class represents a rotation around the z-axis.
   */
  struct JQuaternion3Z  :
    public JQuaternion2D
  {
    /**
     * Default constructor.
     */
    JQuaternion3Z() :
      JQuaternion2D()
    {}

    
    /**
     * Constructor.
     *
     * \param  theta         rotation angle [rad]
     */
    JQuaternion3Z(const double theta) :
      JQuaternion2D(theta)
    {}

    
    /**
     * Get d value.
     *
     * \return               d value
     */
    double getD() const 
    { 
      return __u;
    }
  };
  

  /**
   * Data structure for quaternion in three dimensions.
   *
   * This class implements the JMATH::JMath and JLANG::JEquals interfaces.
   */
  class JQuaternion3D :
    public JMath  <JQuaternion3D>,
    public JMath  <JQuaternion3D, JQuaternion3X>,
    public JMath  <JQuaternion3D, JQuaternion3Y>,
    public JMath  <JQuaternion3D, JQuaternion3Z>,
    public JEquals<JQuaternion3D>
  {
  public:

    using JMath<JQuaternion3D>::mul;
    using JMath<JQuaternion3D, JQuaternion3X>::mul;
    using JMath<JQuaternion3D, JQuaternion3Y>::mul;
    using JMath<JQuaternion3D, JQuaternion3Z>::mul;
    
    
    /**
     * Default constructor.
     */
    JQuaternion3D() :
      __a(0.0),
      __b(0.0),
      __c(0.0),
      __d(0.0)
    {}


    /**
     * Constructor.
     *
     * \param  a             a value
     * \param  b             b value
     * \param  c             c value
     * \param  d             d value
     */
    JQuaternion3D(const double a,
                  const double b,
                  const double c,
                  const double d) :
      __a(a),
      __b(b),
      __c(c),
      __d(d)
    {}


    /**
     * Constructor.
     *
     * This constructor represents a rotation around the given axis by the given angle.
     *
     * \param  theta         rotation angle [rad]
     * \param  dir           rotation axis
     */
    JQuaternion3D(const double     theta,
                  const JVersor3D& dir)
    {
      const double ct = cos(0.5*theta);
      const double st = sin(0.5*theta);

      __a = ct;
      __b = st * dir.getDX();
      __c = st * dir.getDY();
      __d = st * dir.getDZ();
    }


    /**
     * Constructor.
     *
     * \param  qx            rotation around x-axis
     * \param  qy            rotation around y-axis
     * \param  qz            rotation around x-axis
     */
    JQuaternion3D(const JQuaternion3X& qx,
                  const JQuaternion3Y& qy,
                  const JQuaternion3Z& qz) :
      __a(1.0),
      __b(0.0),
      __c(0.0),
      __d(0.0)
    {
      mul(qz).mul(qy).mul(qx);
    }


    /**
     * Get quaternion.
     *
     * \return               quaternion
     */
    const JQuaternion3D& getQuaternion() const
    {
      return static_cast<const JQuaternion3D&>(*this);
    }


    /**
     * Get quaternion.
     *
     * \return               quaternion
     */
    JQuaternion3D& getQuaternion()
    {
      return static_cast<JQuaternion3D&>(*this);
    }


    /**
     * Set quaternion.
     *
     * \param  quaternion    quaternion
     */
    void setQuaternion(const JQuaternion3D& quaternion)
    {
      static_cast<JQuaternion3D&>(*this) = quaternion;
    }

    
    /**
     * Type conversion operator.
     *
     * \return               position
     */
    operator JVector3D() const
    {
      return JVector3D(this->getB(), this->getC(), this->getD());
    }

    
    /**
     * Type conversion operator.
     *
     * \return               direction
     */
    operator JVersor3D() const
    {
      return JVersor3D(this->getB(), this->getC(), this->getD());
    }


    /**
     * Get a value.
     *
     * \return               a value
     */
    double getA() const 
    { 
      return __a;
    }

    
    /**
     * Get b value.
     *
     * \return               b value
     */
    double getB() const 
    { 
      return __b;
    }


    /**
     * Get c value.
     *
     * \return               c value
     */
    double getC() const 
    { 
      return __c;
    }


    /**
     * Get d value.
     *
     * \return               d value
     */
    double getD() const 
    { 
      return __d;
    }


    /**
     * Set to identity matrix
     *
     * \return               this quaternion
     */
    JQuaternion3D& setIdentity()
    {
      __a = 1.0;
      __b = 0.0;
      __c = 0.0;
      __d = 0.0;

      return *this;
    }


    /** 
     * Conjugate quaternion.
     *
     * \return               this quaternion
     */
    JQuaternion3D& conjugate()
    {
      __b = -__b;
      __c = -__c;
      __d = -__d;

      return *this;
    }


    /** 
     * Negate quaternion.
     *
     * \return               this quaternion
     */
    JQuaternion3D& negate()
    {
      __a = -__a;
      __b = -__b;
      __c = -__c;
      __d = -__d;

      return *this;
    }


    /** 
     * Add quaternion.
     *
     * \param  quaternion    quaternion
     * \return               this quaternion
     */
    JQuaternion3D& add(const JQuaternion3D& quaternion)
    {
      __a += quaternion.getA();
      __b += quaternion.getB();
      __c += quaternion.getC();
      __d += quaternion.getD();

      return *this;
    }


    /** 
     * Subtract quaternion.
     *
     * \param  quaternion    quaternion
     * \return               this quaternion
     */
    JQuaternion3D& sub(const JQuaternion3D& quaternion)
    {
      __a -= quaternion.getA();
      __b -= quaternion.getB();
      __c -= quaternion.getC();
      __d -= quaternion.getD();

      return *this;
    }


    /** 
     * Scale quaternion.
     *
     * \param  factor        multiplication factor
     * \return               this quaternion
     */
    JQuaternion3D& mul(const double factor)
    {
      __a *= factor;
      __b *= factor;
      __c *= factor;
      __d *= factor;

      return *this;
    }


    /** 
     * Scale quaternion.
     *
     * \param  factor        division factor
     * \return               this quaternion
     */
    JQuaternion3D& div(const double factor)
    {
      __a /= factor;
      __b /= factor;
      __c /= factor;
      __d /= factor;

      return *this;
    }


    /**
     * Quaternion multiplicaion.
     *
     * This method evaluates the Hamilton product (also called cross product).
     *
     * \param  first         first  quaternion
     * \param  second        second quaternion
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3D& first, const JQuaternion3X& second)
    {
      __a =  first.getA() * second.getA() - first.getB() * second.getB();
      __b =  first.getA() * second.getB() + first.getB() * second.getA();
      __c =  first.getC() * second.getA() + first.getD() * second.getB();
      __d = -first.getC() * second.getB() + first.getD() * second.getA();
      
      return *this;
    }


    /**
     * Quaternion multiplicaion.
     *
     * This method evaluates the Hamilton product (or cross product).
     *
     * \param  first         first  quaternion
     * \param  second        second quaternion
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3D& first,
                       const JQuaternion3Y& second)
    {
      __a =  first.getA() * second.getA() - first.getC() * second.getC();
      __b =  first.getB() * second.getA() - first.getD() * second.getC();
      __c =  first.getA() * second.getC() + first.getC() * second.getA();
      __d =  first.getB() * second.getC() + first.getD() * second.getA();
      
      return *this;
    }


    /**
     * Quaternion multiplicaion.
     *
     * This method evaluates the Hamilton product (or cross product).
     *
     * \param  first         first  quaternion
     * \param  second        second quaternion
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3D& first,
                       const JQuaternion3Z& second)
    {
      __a =  first.getA() * second.getA() - first.getD() * second.getD();
      __b =  first.getB() * second.getA() + first.getC() * second.getD();
      __c = -first.getB() * second.getD() + first.getC() * second.getA();
      __d =  first.getA() * second.getD() + first.getD() * second.getA();
      
      return *this;
    }


    /**
     * Quaternion multiplicaion.
     *
     * This method evaluates the Hamilton product (or cross product).
     *
     * \param  first         first  quaternion
     * \param  second        second quaternion
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3D& first,
                       const JQuaternion3D& second)
    {
      __a = first.getA() * second.getA() - first.getB() * second.getB() - first.getC() * second.getC() - first.getD() * second.getD();
      __b = first.getA() * second.getB() + first.getB() * second.getA() + first.getC() * second.getD() - first.getD() * second.getC();
      __c = first.getA() * second.getC() - first.getB() * second.getD() + first.getC() * second.getA() + first.getD() * second.getB();
      __d = first.getA() * second.getD() + first.getB() * second.getC() - first.getC() * second.getB() + first.getD() * second.getA();
      
      return *this;
    }


    /**
     * Quaternion multiplicaion.
     *
     * \param  qx            rotation around x-axis
     * \param  qy            rotation around y-axis
     * \param  qz            rotation around x-axis
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3X& qx,
                       const JQuaternion3Y& qy,
                       const JQuaternion3Z& qz)
    {
      return *this = JQuaternion3D().setIdentity().mul(qz).mul(qy).mul(qx);
    }


    /**
     * Rotate.
     *
     * \param  __x      x value
     * \param  __y      y value
     * \param  __z      z value
     */
    void rotate(double& __x, double& __y, double& __z) const
    {
      const double qx = 2.0 * (__c*__z - __d*__y);
      const double qy = 2.0 * (__d*__x - __b*__z);
      const double qz = 2.0 * (__b*__y - __c*__x);

      const double x  = __x + __c*qz - __d*qy + __a*qx;
      const double y  = __y - __b*qz + __a*qy + __d*qx;
      const double z  = __z + __a*qz + __b*qy - __c*qx;

      __x = x;
      __y = y;
      __z = z;
    }


    /**
     * Rotate back.
     *
     * \param  __x      x value
     * \param  __y      y value
     * \param  __z      z value
     */
    void rotate_back(double& __x, double& __y, double& __z) const
    {
      const double qx = 2.0 * (__d*__y - __c*__z);
      const double qy = 2.0 * (__b*__z - __d*__x);
      const double qz = 2.0 * (__c*__x - __b*__y);

      const double x  = __x - __c*qz + __d*qy + __a*qx;
      const double y  = __y + __b*qz + __a*qy - __d*qx;
      const double z  = __z + __a*qz - __b*qy + __c*qx;

      __x = x;
      __y = y;
      __z = z;
    }
    

    /** 
     * Check equality.
     *
     * \param  quaternion    quaternion
     * \param  precision     numerical precision
     * \return               true if quaternions are equal; else false
     */
    bool equals(const JQuaternion3D& quaternion,
                const double         precision = std::numeric_limits<double>::min()) const
    {
      return (fabs(getA() - quaternion.getA()) <= precision && 
              fabs(getB() - quaternion.getB()) <= precision &&
              fabs(getC() - quaternion.getC()) <= precision &&
              fabs(getD() - quaternion.getD()) <= precision);
    }


    /**
     * Get length squared.
     *
     * \return               square of length
     */
    double getLengthSquared() const 
    { 
      return getA()*getA() + getB()*getB() + getC()*getC() + getD()*getD();
    }


    /**
     * Get length.
     *
     * \return               length
     */
    double getLength() const 
    { 
      return sqrt(getLengthSquared());
    }


    /**
     * Get squared of distance to quaternion.
     *
     * \param  quaternion    quaternion
     * \return               square of distance
     */
    double getDistanceSquared(const JQuaternion3D& quaternion) const
    {
      return JQuaternion3D(quaternion).sub(*this).getLengthSquared();
    }


    /**
     * Get distance to quaternion.
     *
     * \param  quaternion    quaternion
     * \return               distance
     */
    double getDistance(const JQuaternion3D& quaternion) const
    {
      return sqrt(getDistanceSquared(quaternion));
    }


    /**
     * Get dot product.
     *
     * \param  quaternion    quaternion
     * \return               dot product
     */
    double getDot(const JQuaternion3D& quaternion) const
    {
      return
        getA() * quaternion.getA() -
        getB() * quaternion.getB() -
        getC() * quaternion.getC() -
        getD() * quaternion.getD();
    }


    /**
     * Write quaternion from input.
     *
     * \param  in            input stream
     * \param  quaternion    quaternion
     * \return               input stream
     */
    friend inline std::istream& operator>>(std::istream& in, JQuaternion3D& quaternion)
    {
      in >> quaternion.__a;
      in >> quaternion.__b;
      in >> quaternion.__c;
      in >> quaternion.__d;

      return in;
    }


    /**
     * Write quaternion to output.
     *
     * \param  out           output stream
     * \param  quaternion    quaternion
     * \return               output stream
     */
    friend inline std::ostream& operator<<(std::ostream& out, const JQuaternion3D& quaternion)
    {
      out << quaternion.getA() << ' '
          << quaternion.getB() << ' '
          << quaternion.getC() << ' '
          << quaternion.getD();

      return out;
    }
    

    /**
     * Read quaternion from input.
     *
     * \param  in            reader
     * \param  quaternion    quaternion
     * \return               reader
     */
    friend inline JReader& operator>>(JReader& in, JQuaternion3D& quaternion)
    {
      in >> quaternion.__a;
      in >> quaternion.__b;
      in >> quaternion.__c;
      in >> quaternion.__d;

      return in;
    }


    /**
     * Write quaternion to output.
     *
     * \param  out           writer
     * \param  quaternion    quaternion
     * \return               writer
     */
    friend inline JWriter& operator<<(JWriter& out, const JQuaternion3D& quaternion)
    {
      out << quaternion.getA();
      out << quaternion.getB();
      out << quaternion.getC();
      out << quaternion.getD();

      return out;
    }


    /**
     * Normalise quaternion.
     *
     * \return               this quaternion
     */
    JQuaternion3D& normalise()
    {
      const double v = getLength();

      if (v != 0.0) {
        mul(1.0 / v);
      }

      return *this;
    }

  protected:
    double __a;
    double __b;
    double __c;
    double __d;
  };
}

#endif