RigidTransformN.hpp

//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2011
//
//============================================================================

#ifndef __Thea_RigidTransformN_hpp__
#define __Thea_RigidTransformN_hpp__

#include "Common.hpp"
#include "AffineTransformN.hpp"
#include <sstream>

namespace Thea {

// Forward declarations
template <int N, typename T> class RigidTransformN;

namespace Internal {

template <int N, typename T>
class /* THEA_DLL_LOCAL */ RigidTransformNBase
{
  public:
    typedef RigidTransformN<N, T>   RigidTransformT;   
    typedef AffineTransformN<N, T>  AffineTransformT;  
    typedef Vector<N, T>            VectorT;           
    typedef Matrix<N, N, T>         MatrixT;           

    THEA_DECL_SMART_POINTERS(RigidTransformT)

  public:
    RigidTransformNBase() : aff(AffineTransformT::identity()) {}

    static RigidTransformT translation(VectorT const & translation_)
    {
      return _fromAffine(AffineTransformT::translation(translation_));
    }

    static RigidTransformT _fromAffine(AffineTransformT const & aff_)
    {
      RigidTransformT rt;
      rt.aff = aff_;
      return rt;
    }

    template <typename U> RigidTransformN<N, U> cast() const
    {
      return RigidTransformN<N, U>::_fromAffine(aff.template cast<U>());
    }

    void setIdentity() { *this = identity(); }

    MatrixT const & getRotation() const { return aff.getLinear(); }

    void _setRotation(MatrixT const & rot) { aff.setLinear(rot); }

    VectorT const & getTranslation() const { return aff.getTranslation(); }

    void setTranslation(VectorT const & translation_) { aff.setTranslation(translation_); }

    Matrix<N, N + 1, T> toMatrix() const { return aff.toMatrix(); }

    Matrix<N + 1, N + 1, T> homogeneous() const { return aff.homogeneous(); }

    AffineTransformT const & toAffine() const { return aff; }

    operator AffineTransformT() const { return aff; }

    RigidTransformT inverse() const
    {
      MatrixT inv = aff.getLinear().transpose();  // transpose == inverse for orthogonal matrix
      return _fromAffine(AffineTransformT(inv, inv * (-aff.getTranslation())));
    }

    Real operator()(int i, int j) const { return aff(i, j); }

    RigidTransformT operator*(RigidTransformT const & rhs) const { return _fromAffine(aff * rhs.aff); }

    VectorT operator*(VectorT const & v) const { return aff * v; }

    std::string toString() const
    {
      std::ostringstream oss;
      oss << "[R: " << Thea::toString(getRotation()) << ", T: " << Thea::toString(getTranslation()) << ']';
      return oss.str();
    }

    static RigidTransformT const & identity()
    {
      static RigidTransformT const idty = _fromAffine(AffineTransformT::identity());
      return idty;
    }

  private:
    AffineTransformT aff;  

}; // class RigidTransformNBase

} // namespace Internal

template <int N, typename T = Real>
class /* THEA_API */ RigidTransformN : public Internal::RigidTransformNBase<N, T>
{
  private:
    typedef Internal::RigidTransformNBase<N, T> BaseT;

  public:
    typedef typename BaseT::AffineTransformT  AffineTransformT;
    typedef typename BaseT::VectorT           VectorT;
    typedef typename BaseT::MatrixT           MatrixT;

    RigidTransformN() {}

}; // class RigidTransformN

template <int N, typename T>
std::ostream &
operator<<(std::ostream & os, RigidTransformN<N, T> const & tr)
{
  return os << tr.toString();
}

} // namespace Thea

#include "RigidTransform2.hpp"
#include "RigidTransform3.hpp"

#endif