JRotation3D.hh

Go to the documentation of this file.

#ifndef __JROTATION3D__
#define __JROTATION3D__

#include <cmath>

#include "JGeometry3D/JMatrix3D.hh"
#include "JGeometry3D/JAngle3D.hh"
#include "JGeometry3D/JVersor3D.hh"
#include "JGeometry3D/JVersor3Z.hh"
#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry2D/JRotation2D.hh"
#include "JMath/JMath.hh"


/**
 * \author mdejong
 */

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

namespace JGEOMETRY3D {

  using JGEOMETRY2D::JRotation2D;
  using JMATH::JMath;
  

  /**
   * Rotation around X-axis.
   */
  class JRotation3X :
    public JRotation2D
  {
  public:
    /**
     * Default constructor (= identity matrix).
     */
    JRotation3X() :
      JRotation2D()
    {}


    /**
     * Constructor.
     *
     * \param  phi        rotation angle (anti-clock wise) [rad]
     */
    JRotation3X(const double phi) :
      JRotation2D(phi)
    {}
  };


  /**
   * Rotation around Y-axis.
   */
  class JRotation3Y :
    public JRotation2D
  {
  public:
    /**
     * Default constructor (= identity matrix).
     */
    JRotation3Y() :
      JRotation2D()
    {}


    /**
     * Constructor.
     *
     * Note that in the x-z plane the corresponding rotation is clock wise.
     *
     * \param  phi        rotation angle (anti-clock wise) [rad]
     */
    JRotation3Y(const double phi) :
      JRotation2D(-phi)
    {}
  };


  /**
   * Rotation around Z-axis.
   */
  class JRotation3Z :
    public JRotation2D
  {
  public:
    /**
     * Default constructor (= identity matrix).
     */
    JRotation3Z() :
      JRotation2D()
    {}


    /**
     * Constructor.
     *
     * \param  phi        rotation angle (anti-clock wise) [rad]
     */
    JRotation3Z(const double phi) :
      JRotation2D(phi)
    {}
  };


  /**
   * Rotation matrix
   */
  class JRotation3D :
    public JMatrix3D,
    public JMath<JRotation3D>
  {
  public:

    using JMath<JRotation3D>::mul;

    /**
     * Default constructor (= identity matrix).
     */
    JRotation3D() :
      JMatrix3D()
    {
      setIdentity();
    }


    /**
     * Constructor.
     *
     * The matrix is defined such that the rotation of a vector in the given direction ends up along the z-axis
     * and the back rotation of a vector parallel to the z-axis ends up in the given direction.
     * 
     * \param  dir        direction
     */
    JRotation3D(const JAngle3D& dir) :
      JMatrix3D()
    {
      const double ct = cos(dir.getTheta());
      const double st = sin(dir.getTheta());
      const double cp = cos(dir.getPhi());
      const double sp = sin(dir.getPhi());

      a00 =  ct*cp;  a01 =  ct*sp;  a02 = -st;
      a10 = -sp;     a11 =  cp;     a12 = 0.0;
      a20 =  st*cp;  a21 =  st*sp;  a22 = +ct;
    }


    /**
     * Constructor.
     *
     * The matrix is defined such that the rotation of a vector in the given direction ends up along the z-axis
     * and the back rotation of a vector parallel to the z-axis ends up in the given direction.
     * 
     * \param  dir        direction
     */
    JRotation3D(const JVersor3D& dir) :
      JMatrix3D()
    {
      const double ct  = dir.getDZ();
      const double st  = sqrt((1.0 + ct)*(1.0 - ct));
      const double phi = atan2(dir.getDY(), dir.getDX());
      const double cp  = cos(phi);
      const double sp  = sin(phi);

      a00 =  ct*cp;  a01 =  ct*sp;  a02 = -st;
      a10 = -sp;     a11 =  cp;     a12 = 0.0;
      a20 =  st*cp;  a21 =  st*sp;  a22 = +ct;
    }


    /**
     * Constructor.
     *
     * The matrix is defined such that the rotation of a vector in the given direction ends up along the z-axis
     * and the back rotation of a vector parallel to the z-axis ends up in the given direction.
     * 
     * \param  dir        direction
     */
    JRotation3D(const JVersor3Z& dir) :
      JMatrix3D()
    {
      const double ct  = dir.getDZ();
      const double st  = sqrt((1.0 + ct)*(1.0 - ct));
      const double phi = atan2(dir.getDY(), dir.getDX());
      const double cp  = cos(phi);
      const double sp  = sin(phi);

      a00 =  ct*cp;  a01 =  ct*sp;  a02 = -st;
      a10 = -sp;     a11 =  cp;     a12 = 0.0;
      a20 =  st*cp;  a21 =  st*sp;  a22 = +ct;
    }


    /**
     * Constructor.
     * 
     * \param  R          2D rotation matrix around X-axis
     */
    JRotation3D(const JRotation3X& R) :
      JMatrix3D()
    {
      a00 =  1.0;   a01 =  0.0;     a02 =  0.0;
      a10 =  0.0;   a11 =  R.a00;   a12 =  R.a01;
      a20 =  0.0;   a21 =  R.a10;   a22 =  R.a11;
    }


    /**
     * Constructor.
     * 
     * \param  R          2D rotation matrix around Y-axis
     */
    JRotation3D(const JRotation3Y& R) :
      JMatrix3D()
    {
      a00 =  R.a00;  a01 =  0.0;    a02 =  R.a01;
      a10 =  0.0;    a11 =  1.0;    a12 =  0.0;
      a20 =  R.a10;  a21 =  0.0;    a22 =  R.a11;
    }


    /**
     * Constructor.
     * 
     * \param  R          2D rotation matrix around Z-axis
     */
    JRotation3D(const JRotation3Z& R) :
      JMatrix3D()
    {
      a00 =  R.a00;  a01 =  R.a01;  a02 =  0.0;
      a10 =  R.a10;  a11 =  R.a11;  a12 =  0.0;
      a20 =  0.0;    a21 =  0.0;    a22 =  1.0;
    }


    /**
     * Constructor.
     * 
     * \param  Q          quaternion
     */
    JRotation3D(const JQuaternion3D& Q) :
      JMatrix3D()
    {
      const double a2 = Q.getA()*Q.getA();
      const double b2 = Q.getB()*Q.getB();
      const double c2 = Q.getC()*Q.getC();
      const double d2 = Q.getD()*Q.getD();

      const double ab = Q.getA()*Q.getB();
      const double ac = Q.getA()*Q.getC();
      const double ad = Q.getA()*Q.getD();

      const double bc = Q.getB()*Q.getC();
      const double bd = Q.getB()*Q.getD();

      const double cd = Q.getC()*Q.getD();

      a00 =  a2 + b2 - c2 - d2;  a01 =   2.0*bc - 2.0*ad;   a02 =   2.0*bd + 2.0*ac;
      a10 =   2.0*bc + 2.0*ad;   a11 =  a2 - b2 + c2 - d2;  a12 =   2.0*cd - 2.0*ab;
      a20 =   2.0*bd - 2.0*ac;   a21 =   2.0*cd + 2.0*ab;   a22 =  a2 - b2 - c2 + d2;
    }


    /**
     * Get rotation.
     *
     * \return            rotation
     */
    const JRotation3D& getRotation() const
    {
      return static_cast<const JRotation3D&>(*this);
    }


    /**
     * Type conversion operator.
     *
     * \return               quaternion
     */
    operator JQuaternion3D() const
    {
      const double q2 = 0.25 * (1.0 + a00 + a11 + a22);

      if (q2 > 0.0) {

        const double a =  sqrt(q2);
        const double w =  0.25 / a;
        const double b = (a21 - a12) * w;
        const double c = (a02 - a20) * w;
        const double d = (a10 - a01) * w;
        
        return JQuaternion3D(a,b,c,d).normalise();
      }

      return JQuaternion3D(1.0, 0.0, 0.0, 0.0);
    }

    
    /**
     * Transpose.
     */
    JRotation3D& transpose()
    {
      static_cast<JMatrix3D&>(*this).transpose();

      return *this;
    }

    
    /**
     * Matrix multiplication.
     *
     * \param  A        matrix
     * \param  B        matrix
     * \return          this matrix
     */
    JRotation3D& mul(const JRotation3D& A,
                     const JRotation3D& B)
    {
      static_cast<JMatrix3D&>(*this).mul(A, B);

      return *this;
    }      

    
    /**
     * Rotate.
     *
     * \param  __x      x value
     * \param  __y      y value
     * \param  __z      z value
     */
    void rotate(double& __x, double& __y, double& __z) const
    {
      const double x = a00 * __x  +  a01 * __y  +  a02 * __z;
      const double y = a10 * __x  +  a11 * __y  +  a12 * __z;
      const double z = a20 * __x  +  a21 * __y  +  a22 * __z;

      __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 x = a00 * __x  +  a10 * __y  +  a20 * __z;
      const double y = a01 * __x  +  a11 * __y  +  a21 * __z;
      const double z = a02 * __x  +  a12 * __y  +  a22 * __z;

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

#endif