#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__
