path: root/meowpp/math/LinearTransformations.h

#ifndef   math_LinearTransformations_H__
#define   math_LinearTransformations_H__

#include "LinearTransformation.h"
#include "Matrix.h"
#include "utility.h"
#include "../Self.h"
#include "../geo/Vectors.h"

#include <cstdlib>

namespace meow {

/*!
 * @brief Rotation a point/vector alone an axis with given angle in 3D world.
 *
 * @author cat_leopard
 */
template<class Scalar>
class Rotation3D: public LinearTransformation<Scalar> {
private:
  struct Myself {
    Vector3D<Scalar> theta_;
    bool              need_;
    
    Myself(): theta_(0, 0, 0), need_(true) {
    }
    
    Myself(Myself const& b): theta_(b.theta_), need_(b.need_) {
    }
    
    ~Myself() {
    }
  };
  
  Self<Myself> const self;
  
  void calcMatrix() const {
    if (self->need_) {
      Matrix<Scalar> tmp(3, 3, 0.0);
      if (noEPS(self->theta_.length2()) == 0) {
        tmp.identitied();
      }
      else {
        Vector3D<double> axis (self->theta_.normalize());
        double           angle(self->theta_.length());
        double           cs(cos(angle / 2.0));
        double           sn(sin(angle / 2.0));

        tmp.entry(0, 0, 2*(squ(axis.x())-1.0)*squ(sn) + 1);
        tmp.entry(1, 1, 2*(squ(axis.y())-1.0)*squ(sn) + 1);
        tmp.entry(2, 2, 2*(squ(axis.z())-1.0)*squ(sn) + 1);
        tmp.entry(0, 1, 2*axis.x()*axis.y()*squ(sn) - 2*axis.z()*cs*sn);
        tmp.entry(1, 0, 2*axis.y()*axis.x()*squ(sn) + 2*axis.z()*cs*sn);
        tmp.entry(0, 2, 2*axis.x()*axis.z()*squ(sn) + 2*axis.y()*cs*sn);
        tmp.entry(2, 0, 2*axis.z()*axis.x()*squ(sn) - 2*axis.y()*cs*sn);
        tmp.entry(1, 2, 2*axis.y()*axis.z()*squ(sn) - 2*axis.x()*cs*sn);
        tmp.entry(2, 1, 2*axis.z()*axis.y()*squ(sn) + 2*axis.x()*cs*sn);
      }
      ((Rotation3D*)this)->LinearTransformation<Scalar>::matrix(tmp);
      self()->need_ = false;
    }
  }
  
public:
  /*!
   * Constructor with no rotation
   */
  Rotation3D(): LinearTransformation<Scalar>(3u, 3u, 3u), self() {
  }
  
  /*!
   * Constructor and copy data
   */
  Rotation3D(Rotation3D const& b): LinearTransformation<Scalar>(b),
  self(b.self, Self<Myself>::COPY_FROM) {
  }
  
  /*!
   * Destructor
   */
  ~Rotation3D() {
  }
  
  /*!
   * @brief Copy data
   *
   * @param [in] b another Rotation3D class.
   * @return \c *this
   */
  Rotation3D& copyFrom(Rotation3D const& b) {
    LinearTransformation<Scalar>::copyFrom(b);
    self().copyFrom(b.self);
    return *this;
  }
  
  /*!
   * @brief Reference data
   *
   * @param [in] b another Rotation3D class.
   * @return \c *this
   */
  Rotation3D& referenceFrom(Rotation3D const& b) {
    LinearTransformation<Scalar>::referenceFrom(b);
    self().referenceFrom(b.self);
    return *this;
  }

  /*!
   * @brief same as \c theta(i)
   */
  Scalar parameter(size_t i) const {
    return theta(i);
  }

  /*!
   * @brief same as \c theta(i, s)
   */
  Scalar parameter(size_t i, Scalar const& s) {
    return theta(i, s);
  }
  
  /*!
   * @brief Get the \c i -th theta
   *
   * \c i can only be 1, 2 or 3
   *
   * @param [in] i index
   * @return \c i -th theta
   */
  Scalar const& theta(size_t i) const {
    return self->theta_(i);
  }
  
  /*!
   * @brief Set the \c i -th theta
   *
   * \c i can only be 1, 2 or 3
   *
   * @param [in] i index
   * @param [in] s new theta value
   * @return \c i -th theta
   */
  Scalar const& theta(size_t i, Scalar const& s) {
    if (theta(i) != s) {
      if      (i == 0) self()->theta_.x(s);
      else if (i == 1) self()->theta_.y(s);
      else if (i == 2) self()->theta_.z(s);
      self()->need_ = true;
    }
    return theta(i);
  }

  /*!
   * @brief Setting
   *
   * @param [in] axis  axis
   * @param [in] angle angle
   */
  void axisAngle(Vector<Scalar> const& axis, Scalar const& angle) {
    Vector<Scalar> n(axis.normalize());
    for (size_t i = 0; i < 3; i++) {
      theta(i, n(i) * angle);
    }
  }
  
  /*!
   * @brief Concat another rotation transformation
   * @param [in] r another rotation transformation
   */
  Rotation3D& add(Rotation3D const& r) {
    for (size_t i = 0; i < 3; i++) {
      theta(i, r.theta(i));
    }
    return *this;
  }
  
  /*!
   * @brief Do the transformate
   
   * Assume:
   *  - The input  vector is \f$ (x ,y ,z ) \f$
   *  - The output vector is \f$ (x',y',z') \f$
   *  - The parameters theta is\f$ \vec{\theta}=(\theta_x,\theta_y,\theta_z) \f$
   *  .
   * Then we have:
   * \f[
   *   \left[ \begin{array}{c} x' \\ y' \\ z' \\ \end{array} \right]
   *   =
   *   \left[ \begin{array}{ccc}
   *     2(n_x^2 - 1) \sin^2\phi + 1                  &
   *     2n_x n_y \sin^2\phi - 2n_z\cos \phi\sin \phi &
   *     2n_x n_z \sin^2\phi + 2n_y\cos \phi\sin \phi \\
   *     2n_y n_x \sin^2\phi + 2n_z\cos \phi\sin \phi &
   *     2(n_y^2 - 1) \sin^2\phi + 1                  &
   *     2n_y n_z \sin^2\phi - 2n_x\cos \phi\sin \phi \\
   *     2n_z n_x \sin^2\phi - 2n_y\cos \phi\sin \phi &
   *     2n_z n_y \sin^2\phi + 2n_x\cos \phi\sin \phi &
   *     2(n_z^2 - 1) \sin^2\phi + 1                  \\
   *   \end{array} \right]
   *   \left[ \begin{array}{c} x \\ y \\ z \\ \end{array} \right]
   * \f]
   * Where:
   *  - \f$ \phi \f$ is the helf of length of \f$ \vec{\theta} \f$ ,
   *    which means \f$ \phi = \frac{\left|\vec{\theta}\right|}{2}
   *           = \frac{1}{2}\sqrt{\theta_x^2 + \theta_y^2 + \theta_z^2} \f$
   *  - \f$ \vec{n} \f$ is the normalized form of \f$ \vec{\theta} \f$ ,
   *    which means \f$ \vec{n} = (n_x,n_y,n_z) = \vec{\theta} / 2\phi \f$
   *
   * @param [in] x the input vector
   * @return the output matrix
   */
  Matrix<Scalar> transformate(Matrix<Scalar> const& x) const {
    calcMatrix();
    return LinearTransformation<Scalar>::matrix() * x;
  }
  
  /*!
   * @brief Return the jacobian matrix (derivate by the input vector)
   *        of this transformate
   *
   * The matrix we return is:
   * \f[
   *   \left[ \begin{array}{ccc}
   *     2(n_x^2 - 1) \sin^2\phi + 1                  &
   *     2n_x n_y \sin^2\phi - 2n_z\cos \phi\sin \phi &
   *     2n_x n_z \sin^2\phi + 2n_y\cos \phi\sin \phi \\
   *     2n_y n_x \sin^2\phi + 2n_z\cos \phi\sin \phi &
   *     2(n_y^2 - 1) \sin^2\phi + 1                  &
   *     2n_y n_z \sin^2\phi - 2n_x\cos \phi\sin \phi \\
   *     2n_z n_x \sin^2\phi - 2n_y\cos \phi\sin \phi &
   *     2n_z n_y \sin^2\phi + 2n_x\cos \phi\sin \phi &
   *     2(n_z^2 - 1) \sin^2\phi + 1                  \\
   *   \end{array} \right]
   * \f]
   * Where the definition of \f$ \vec{n} \f$ and \f$ \phi \f$
   * is the same as the definition in the description of
   * the method \b transformate() .
   *
   * @param [in] x the input vector (in this case it is a useless parameter)
   * @return a matrix
   */
  Matrix<Scalar> jacobian(Matrix<Scalar> const& x) const {
    calcMatrix();
    return LinearTransformation<Scalar>::matrix();
  }
  
  /*!
   * @brief Return the jacobian matrix of this transformate
   *
   * Here we need to discussion in three case:
   *  - \a i = 0, derivate by the x axis of the vector theta
   *   \f[
   *     \left[ \begin{array}{ccc}
   *        0 &  0 &  0 \\
   *        0 &  0 & -1 \\
   *        0 &  1 &  0 \\
   *     \end{array} \right]
   *     \left[ \begin{array}{ccc}
   *     2(n_x^2 - 1) \sin^2\phi + 1                  &
   *     2n_x n_y \sin^2\phi - 2n_z\cos \phi\sin \phi &
   *     2n_x n_z \sin^2\phi + 2n_y\cos \phi\sin \phi \\
   *     2n_y n_x \sin^2\phi + 2n_z\cos \phi\sin \phi &
   *     2(n_y^2 - 1) \sin^2\phi + 1                  &
   *     2n_y n_z \sin^2\phi - 2n_x\cos \phi\sin \phi \\
   *     2n_z n_x \sin^2\phi - 2n_y\cos \phi\sin \phi &
   *     2n_z n_y \sin^2\phi + 2n_x\cos \phi\sin \phi &
   *     2(n_z^2 - 1) \sin^2\phi + 1                  \\
   *     \end{array} \right]
   *     \left[ \begin{array}{c} x \\ y \\ z \\ \end{array} \right]
   *   \f]
   *  - \a i = 1, derivate by the y axis of the vector theta
   *   \f[
   *     \left[ \begin{array}{ccc}
   *        0 &  0 &  1 \\
   *        0 &  0 &  0 \\
   *       -1 &  0 &  0 \\
   *     \end{array} \right]
   *     \left[ \begin{array}{ccc}
   *     2(n_x^2 - 1) \sin^2\phi + 1                  &
   *     2n_x n_y \sin^2\phi - 2n_z\cos \phi\sin \phi &
   *     2n_x n_z \sin^2\phi + 2n_y\cos \phi\sin \phi \\
   *     2n_y n_x \sin^2\phi + 2n_z\cos \phi\sin \phi &
   *     2(n_y^2 - 1) \sin^2\phi + 1                  &
   *     2n_y n_z \sin^2\phi - 2n_x\cos \phi\sin \phi \\
   *     2n_z n_x \sin^2\phi - 2n_y\cos \phi\sin \phi &
   *     2n_z n_y \sin^2\phi + 2n_x\cos \phi\sin \phi &
   *     2(n_z^2 - 1) \sin^2\phi + 1                  \\
   *     \end{array} \right]
   *     \left[ \begin{array}{c} x \\ y \\ z \\ \end{array} \right]
   *   \f]
   *  - \a i = 2, derivate by the z axis of the vector theta
   *   \f[
   *     \left[ \begin{array}{ccc}
   *        0 & -1 &  0 \\
   *        1 &  0 &  0 \\
   *        0 &  0 &  0 \\
   *     \end{array} \right]
   *     \left[ \begin{array}{ccc}
   *     2(n_x^2 - 1) \sin^2\phi + 1                  &
   *     2n_x n_y \sin^2\phi - 2n_z\cos \phi\sin \phi &
   *     2n_x n_z \sin^2\phi + 2n_y\cos \phi\sin \phi \\
   *     2n_y n_x \sin^2\phi + 2n_z\cos \phi\sin \phi &
   *     2(n_y^2 - 1) \sin^2\phi + 1                  &
   *     2n_y n_z \sin^2\phi - 2n_x\cos \phi\sin \phi \\
   *     2n_z n_x \sin^2\phi - 2n_y\cos \phi\sin \phi &
   *     2n_z n_y \sin^2\phi + 2n_x\cos \phi\sin \phi &
   *     2(n_z^2 - 1) \sin^2\phi + 1                  \\
   *     \end{array} \right]
   *     \left[ \begin{array}{c} x \\ y \\ z \\ \end{array} \right]
   *   \f]
   *  .
   * Where \f$ (x,y,z) \f$ is the input vector, \f$ \vec{n}, \phi \f$ is the
   * same one in the description of \b transformate().
   *
   * @param [in] x the input vector
   * @param [in] i the index of the parameters(theta) to dervite
   * @return a matrix
   */
  Matrix<Scalar> jacobian(Matrix<Scalar> const& x, size_t i) const {
    calcMatrix();
    Matrix<Scalar> mid(3u, 3u, Scalar(0.0));
    if (i == 0) {
      mid.entry(1, 2, Scalar(-1.0));
      mid.entry(2, 1, Scalar( 1.0));
    }
    else if(i == 1) {
      mid.entry(0, 2, Scalar( 1.0));
      mid.entry(2, 0, Scalar(-1.0));
    }
    else {
      mid.entry(0, 1, Scalar(-1.0));
      mid.entry(1, 0, Scalar( 1.0));
    }
    return mid * LinearTransformation<Scalar>::matrix() * x;
  }

  /*!
   * @brief Do the inverse transformate
   *
   * @param [in] x the input vector
   * @return the output vector
   */
  Matrix<Scalar> transformateInv(Matrix<Scalar> const& x) const {
    return matrixInv() * x;
  }
  
  /*!
   * @brief Return the jacobian matrix of the inverse form of this transformate
   *
   * @param [in] x the input vector
   * @return a matrix
   */
  Matrix<Scalar> jacobianInv(Matrix<Scalar> const& x) const {
    return matrixInv();
  }
  
  /*!
   * @brief Return the jacobian matrix of the inverse form of this transformate
   *
   * @param [in] x the input vector
   * @param [in] i the index of the parameters(theta) to dervite
   * @return a matrix
   */
  Matrix<Scalar> jacobianInv(Matrix<Scalar> const& x, size_t i) const {
    calcMatrix();
    Matrix<Scalar> mid(3u, 3u, Scalar(0.0));
    if (i == 0) {
      mid.entry(1, 2, Scalar(-1.0));
      mid.entry(2, 1, Scalar( 1.0));
    }
    else if(i == 1) {
      mid.entry(0, 2, Scalar( 1.0));
      mid.entry(2, 0, Scalar(-1.0));
    }
    else {
      mid.entry(0, 1, Scalar(-1.0));
      mid.entry(1, 0, Scalar( 1.0));
    }
    return matrixInv() * mid.transpose() * x;
    return (-mid) * matrixInv() * x;
  }

  /*!
   * @brief Return the inverse matrix
   *
   * In this case, the inverse matrix is equal to the transpose of the matrix
   *
   * @return a matrix
   */
  Matrix<Scalar> matrixInv() const {
    calcMatrix();
    return LinearTransformation<Scalar>::matrix().transpose();
  }
  
  //! @brief same as \c copyFrom(b)
  Rotation3D& operator=(Rotation3D const& b) {
    return copyFrom(b);
  }
};

}

#endif // math_LinearTransformations_H__