rtcTransform2D.h

Go to the documentation of this file.

/*
 * Copyright (C) 2009
 * Robert Bosch LLC
 * Research and Technology Center North America
 * Palo Alto, California
 *
 * All rights reserved.
 *
 *------------------------------------------------------------------------------
 * project ....: Autonomous Technologies
 * file .......: rtcTransform2D.h
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 02/29/2008
 * modified ...: $Date: 2009-03-23 17:48:44 -0700 (Mon, 23 Mar 2009) $
 * changed by .: $Author: wg75pal $
 * revision ...: $Revision: 158 $
 */
#ifndef RTC_TRANSFORM2D_H
#define RTC_TRANSFORM2D_H

//== INCLUDES ==================================================================
#include "rtc/rtcMath.h"
#include "rtc/rtcTransform2D.h"
#include "rtc/rtcRotation2D.h"
#include "rtc/rtcVec.h"
#include "rtc/rtcVec2.h"
#include "rtc/rtcVec3.h"
#include "rtc/rtcSMat3.h"

//== NAMESPACES ================================================================
namespace rtc {

//== FORWARD DECLARATIONS ======================================================
template <class T> class Transform2D;   // Rigid tranform matrix (3x3)
template <class T> class Rotation2D;    // Rotation2D matrix (2x2)
template <class T, int M> class Vec;    // M-d vector
template <class T> class Vec2;          // 2-d vector

template <class T>
class Transform2D: public SMat3<T> {
public:
  // Constructors
  Transform2D();
  Transform2D(const Mat<T,3,3>& m);
  Transform2D(const SMat3<T>& m);
  Transform2D(const Rotation2D<T>& rot);
  Transform2D(const Vec2<T>& trans);
  Transform2D(const Rotation2D<T>& rot, const Vec2<T>& trans);
  Transform2D(const T& dtheta, const T& dx, const T& dy);

  // Accessors
  void get( Rotation2D<T>& rot, Vec2<T>& trans) const;
  void get(T& theta, T& dx, T& dy) const;
  Vec2<T> getTrans( ) const;
  void getTrans(T& dx, T& dy) const;
  Rotation2D<T> getRot( ) const;
  void getRot(T& dtheta) const;

  // Mutators
  void set(const Rotation2D<T>& rot);
  void set(const Vec2<T>& trans);
  void set(const Rotation2D<T>& rot, const Vec2<T>& trans);
  void set(const T& dtheta, const T& dx, const T& dy);
  void rotate(T theta);
  void translate(T x, T y);
  void translate(const Vec2<T>& t);

  // Helper for applying tranform to points: Transform2D * Vec<T,2>
  Vec<T,3> operator * (const Vec2<T>& v) const;
  void apply(Vec2<T>& v) const;
  Vec2<T> apply(const Vec2<T>& v) const;

  //Special inverse that uses the fact this is a rigid transform matrix
  Transform2D<T> inverted() const;
  Transform2D<T> relativeTo(const Transform2D<T>& referenceFrame) const;

  // Inherited from parent
  using SMat<T,3>::set;
  using SMat<T,3>::x;
#ifndef WIN32
  using SMat<T,3>::operator*;
#endif
};

// Declare a few common typdefs
typedef Transform2D<float> Transform2Df;
typedef Transform2D<double> Transform2Dd;

//==============================================================================
// Transform2D<T>
//==============================================================================

template <class T>
inline Transform2D<T>::Transform2D() {
  set(Rotation2D<T>(),Vec<T,2>(T(0)));
}

template <class T>
inline Transform2D<T>::Transform2D(const Rotation2D<T>& r){
  set(r,Vec2<T>(T(0)));
}

template <class T>
inline Transform2D<T>::Transform2D(const Vec2<T>& t){
  set(Rotation2D<T>(),t);
}

template <class T>
inline Transform2D<T>::Transform2D(const Rotation2D<T>& rot, const Vec2<T>& trans){
  set(rot,trans);
}

template <class T>
inline Transform2D<T>::Transform2D(const SMat3<T>& m) : SMat3<T>(m) {}

template <class T>
inline Transform2D<T>::Transform2D(const T& dtheta, const T& dx, const T& dy){
  set(dtheta,dx,dy);
}

template <class T>
inline Transform2D<T>::Transform2D(const Mat<T,3,3>& m) : SMat3<T>(m) {}

// Accessors

template <class T>
inline void Transform2D<T>::get(Rotation2D<T>& r, Vec2<T>& t) const
{
  r=Rotation2D<T>(x[0],x[1],
                  x[3],x[4]);
  t=Vec2<T>(x[2],x[5]);
}

template <class T>
inline void Transform2D<T>::get(T& theta, T& dx, T& dy) const
{
  getTrans(dx,dy);
  getRot(theta);
}

template <class T>
inline Vec2<T> Transform2D<T>::getTrans() const
{
  return Vec2<T>(x[2],x[5]);
}

template <class T>
inline void Transform2D<T>::getTrans(T& dx, T& dy) const
{
  dx=x[2];
  dy=x[5];
}

template <class T>
inline Rotation2D<T> Transform2D<T>::getRot() const
{
  return Rotation2D<T>(x[0],x[1],
                       x[3],x[4]);
}

template <class T>
inline void Transform2D<T>::getRot(T& dtheta) const
{
  dtheta = atan2(x[3],x[0]);
}

// Mutators

template <class T>
inline void Transform2D<T>::set(const Rotation2D<T>& r) {
  for (int i=0;i<2;i++)
    for (int j=0;j<2;j++)
      (*this)(i,j) = r(i,j);
  x[6] = 0; x[7] = 0; x[8] = 1;
}

template <class T>
inline void Transform2D<T>::set(const Vec2<T>& t) {
  x[2] = t(0);
  x[5] = t(1);
  x[6] = 0; x[7] = 0; x[8] = 1;
}

template <class T>
inline void Transform2D<T>::set(const Rotation2D<T>& r, const Vec2<T>& t) {
  set(r);
  set(t);
}

template <class T>
inline void Transform2D<T>::set(const T& dtheta, const T& dx, const T& dy) {
  set(Rotation2D<T>(dtheta),Vec2<T>(dx,dy));
}

template <class T>
inline void Transform2D<T>::rotate(T theta)
{
  Transform2D<T> temp;
  T ctheta = cos(theta), stheta = sin(theta);
  temp(0,0) = ctheta;
  temp(0,1) = -stheta;
  temp(1,0) = stheta;
  temp(1,1) = ctheta;
  leftMultiply(temp);
}

template <class T>
inline void Transform2D<T>::translate(T _x, T _y)
{
  x[2] += _x;
  x[5] += _y;
}

template <class T>
inline void Transform2D<T>::translate(const Vec2<T>& t)
{
  translate(t[0],t[1]);
}

template <class T>
inline Vec<T,3> Transform2D<T>::operator * (const Vec2<T>& v) const {
  return (*(Mat<T,3,3>*)this)*Vec3<T>(v(0),v(1),T(1.0));
}

template <class T>
inline void Transform2D<T>::apply(Vec2<T>& v) const {
  Vec<T,3> v1 = (*this)*v;
  v.set(v1[0],v1[1]);
}

template <class T>
inline Vec2<T> Transform2D<T>::apply(const Vec2<T>& v) const {
  Vec<T,3> v1 = (*this)*v;
  return(Vec2<T>(v1[0],v1[1]));
}

template <class T>
inline Transform2D<T> Transform2D<T>::inverted() const{
  Rotation2D<T> r(x[0],x[3],
                  x[1],x[4]);
  Vec2<T> t(-x[2],-x[5]);
  return Transform2D<T>(r,r*t);
}

template <class T>
inline Transform2D<T> Transform2D<T>::relativeTo(const Transform2D<T>& referenceFrame) const {
  return SMat3<T>(referenceFrame.inverted()*(*this));
}

//==============================================================================
} // NAMESPACE rtc
//==============================================================================
#endif // RTC_TRANSFORM2D_H
//==============================================================================