diff options
Diffstat (limited to 'meowpp/math/LinearTransformations.h')
| -rw-r--r-- | meowpp/math/LinearTransformations.h | 404 |
1 files changed, 0 insertions, 404 deletions
diff --git a/meowpp/math/LinearTransformations.h b/meowpp/math/LinearTransformations.h deleted file mode 100644 index 4bf9a36..0000000 --- a/meowpp/math/LinearTransformations.h +++ /dev/null @@ -1,404 +0,0 @@ -#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); - } -}; - -} // meow - -#endif // math_LinearTransformations_H__ |