diff options
Diffstat (limited to 'meowpp/math/LinearTransformations.h')
| -rw-r--r-- | meowpp/math/LinearTransformations.h | 424 |
1 files changed, 397 insertions, 27 deletions
diff --git a/meowpp/math/LinearTransformations.h b/meowpp/math/LinearTransformations.h index 4878834..c7295ab 100644 --- a/meowpp/math/LinearTransformations.h +++ b/meowpp/math/LinearTransformations.h @@ -2,37 +2,407 @@ #define math_LinearTransformations_H__ #include "LinearTransformation.h" +#include "Matrix.h" +#include "utility.h" +#include "../Self.h" +#include "../geo/Vectors.h" #include <cstdlib> -namespace meow{ - template<class Scalar> - class Rotation3D: public LinearTransformation<Scalar>{ - private: - Scalar _theta[3]; - void calcMatrix(); - public: - Rotation3D(); - - void axisTheta(Matrix<Scalar> const& __axis, Scalar const& __theta); - - Scalar parameter(size_t __i) const; - Scalar parameter(size_t __i, Scalar const& __s); - - Matrix<Scalar> transformate(Matrix<Scalar> const& __x) const; - Matrix<Scalar> jacobian (Matrix<Scalar> const& __x) const; - Matrix<Scalar> jacobian (Matrix<Scalar> const& __x, - size_t __i) const; - - Matrix<Scalar> invTransformate(Matrix<Scalar> const& __x) const; - Matrix<Scalar> invJacobian (Matrix<Scalar> const& __x) const; - Matrix<Scalar> invJacobian (Matrix<Scalar> const& __x, - size_t __i) const; - - Matrix<Scalar> invMatrix() const; +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() { + } + ~Myself() { + } + Myself& copyFrom(Myself const& b) { + theta_ = b.theta_; + need_ = b.need_; + return *this; + } }; -} + + Self<Myself> const& self; + + void calcMatrix() const { + if (self->need_) { + Vector3D<double> axis (self->theta_.normalize()); + double angle(self->theta_.length()); + double cs(cos(angle / 2.0)); + double sn(sin(angle / 2.0)); + + Matrix<Scalar> tmp(3, 3, Scalar(0.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)->matrix(tmp); + self()->need_ = false; + } + } + +public: + /*! + * Constructor with no rotation + */ + Rotation3D(): LinearTransformation<Scalar>(3u, 3u, 3u), + self(true) { + self()->theta_.x(Scalar(0)); + self()->theta_.y(Scalar(0)); + self()->theta_.z(Scalar(0)); + self()->need_ = true; + calcMatrix(); + } + + /*! + * Constructor and copy data + */ + Rotation3D(Rotation3D const& b): LinearTransformation<Scalar>(b), + self(false) { + copyFrom(b); + } + + /*! + * 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); + } -#include "LinearTransformations.hpp" + /*! + * @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 { + if (self->need_) 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 { + if (self->need_) 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 { + if (self->need_) 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 { + if (self->need_) calcMatrix(); + return LinearTransformation<Scalar>::matrix().transpose() * 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 { + if (self->need_) calcMatrix(); + return LinearTransformation<Scalar>::matrix().transpose(); + } + + /*! + * @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 { + if (self->need_) 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.transpose() * 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 { + if (self->need_) calcMatrix(); + return LinearTransformation<Scalar>::matrix().transpose(); + } + + //! @brief same as \c copyFrom(b) + Rotation3D& operator=(Rotation3D const& b) { + return copyFrom(b); + } +}; + +} #endif // math_LinearTransformations_H__ |