LinearTransformations.h

Go to the documentation of this file.

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

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:
  Rotation3D(): LinearTransformation<Scalar>(3u, 3u, 3u), self() {
  }
  
  Rotation3D(Rotation3D const& b): LinearTransformation<Scalar>(b),
  self(b.self, Self<Myself>::COPY_FROM) {
  }
  
  ~Rotation3D() {
  }
  
  Rotation3D& copyFrom(Rotation3D const& b) {
    LinearTransformation<Scalar>::copyFrom(b);
    self().copyFrom(b.self);
    return *this;
  }
  
  Rotation3D& referenceFrom(Rotation3D const& b) {
    LinearTransformation<Scalar>::referenceFrom(b);
    self().referenceFrom(b.self);
    return *this;
  }

  Scalar parameter(size_t i) const {
    return theta(i);
  }

  Scalar parameter(size_t i, Scalar const& s) {
    return theta(i, s);
  }
  
  Scalar const& theta(size_t i) const {
    return self->theta_(i);
  }
  
  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);
  }

  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);
    }
  }
  
  Rotation3D& add(Rotation3D const& r) {
    for (size_t i = 0; i < 3; i++) {
      theta(i, r.theta(i));
    }
    return *this;
  }
  
  Matrix<Scalar> transformate(Matrix<Scalar> const& x) const {
    calcMatrix();
    return LinearTransformation<Scalar>::matrix() * x;
  }
  
  Matrix<Scalar> jacobian(Matrix<Scalar> const& x) const {
    calcMatrix();
    return LinearTransformation<Scalar>::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;
  }

  Matrix<Scalar> transformateInv(Matrix<Scalar> const& x) const {
    return matrixInv() * x;
  }
  
  Matrix<Scalar> jacobianInv(Matrix<Scalar> const& x) const {
    return matrixInv();
  }
  
  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;
  }

  Matrix<Scalar> matrixInv() const {
    calcMatrix();
    return LinearTransformation<Scalar>::matrix().transpose();
  }
  
  Rotation3D& operator=(Rotation3D const& b) {
    return copyFrom(b);
  }
};

}

#endif // math_LinearTransformations_H__