rtcTransform.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 .......: rtcTransform.h
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 08/16/2006
 * modified ...: $Date: 2009-03-19 15:02:17 -0700 (Thu, 19 Mar 2009) $
 * changed by .: $Author: wg75pal $
 * revision ...: $Revision: 103 $
 */
#ifndef RTC_TRANSFORM_H
#define RTC_TRANSFORM_H

//== INCLUDES ==================================================================
#include "rtc/rtcMath.h"
#include "rtc/rtcRotation.h"
#include "rtc/rtcEulerAngles.h"
#include "rtc/rtcQuaternion.h"
#include "rtc/rtcVec3.h"
#include "rtc/rtcSMat4.h"

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

// Forward declarations
template <class T> class EulerAngles;   // Euler angles
template <class T> class Rotation;      // Rotation matrix (3x3)
template <class T> class Transform;     // Rigid tranform matrix (4x4)
template <class T> class Quaternion;    // Quaternion
template <class T> class Vec3;          // 3d Vector

template <class T>
class Transform: public SMat4<T> {
public:
  // Constructors
  Transform();
  Transform(const Mat<T,4,4>& m);
  Transform(const Rotation<T>& rot);
  Transform(const Vec3<T>& trans);
  Transform(const Rotation<T>& rot, const Vec3<T>& trans);

  // Accessors
  void get( Rotation<T>& rot, Vec3<T>& trans) const;
  Vec3<T> getTranslation() const;
  Rotation<T> getRotation() const;

  // Mutators
  void setRotation(const Rotation<T>& rot);
  void setTranslation(const Vec3<T>& trans);
  void set(const Rotation<T>& rot, const Vec3<T>& trans);
  void rotateX(T theta);
  void rotateY(T theta);
  void rotateZ(T theta);
  void translate(T x, T y, T z);
  void translate(const Vec3<T>& t);

  // Helper for applying tranform to points: Transform * Vec<T,3>
  Vec3<T>& apply(Vec3<T>& v) const;
  Vec3<T> apply(const Vec3<T>& v) const;


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

  // calculate a relative transformation inv(referenceFrame)*this
  Transform<T> relativeTo(const Transform<T>& referenceFrame) const;

  //TODO: figure out how to make << a friend and make this protected
  bool verifyTag(std::string got, std::string expected);

  // Inherited from parent
  using SMat<T,4>::set;
  using SMat<T,4>::x;
};

// Declare a few common typdefs
typedef Transform<float> Transformf;
typedef Transform<double> Transformd;

// ASCII stream IO
template <class T> std::ostream& operator<<(std::ostream& os, const Transform<T>& trans);
template <class T> std::istream& operator>>(std::istream& is, Transform<T>& trans);

//==============================================================================
// Transform<T>
//==============================================================================

template <class T>
inline Transform<T>::Transform() {
  set(Rotation<T>(),Vec<T,3>(T(0)));
}

template <class T>
inline Transform<T>::Transform(const Rotation<T>& r){
  set(r,Vec3<T>(T(0)));
}

template <class T>
inline Transform<T>::Transform(const Vec3<T>& t){
  set(Rotation<T>(),t);
}

template <class T>
inline Transform<T>::Transform(const Rotation<T>& rot, const Vec3<T>& trans){
  set(rot,trans);
}

template <class T>
inline Transform<T>::Transform(const Mat<T,4,4>& m) : SMat4<T>(m) {}


// Accessors

template <class T>
inline void Transform<T>::get(Rotation<T>& r, Vec3<T>& t) const{
  r=Rotation<T>(x[0],x[1],x[2],
                x[4],x[5],x[6],
                x[8],x[9],x[10]);
  t=Vec3<T>(x[3],x[7],x[11]);
}

template <class T>
inline Vec3<T> Transform<T>::getTranslation() const{

  return Vec3<T>(x[3],x[7],x[11]);
}

template <class T>
inline  Rotation<T> Transform<T>::getRotation() const{
  return Rotation<T>(x[0],x[1],x[2],
                x[4],x[5],x[6],
                x[8],x[9],x[10]);
}

// Mutators

template <class T>
inline void Transform<T>::setRotation(const Rotation<T>& r) {
  for (int i=0;i<3;i++) for (int j=0;j<3;j++) (*this)(i,j) = r(i,j);
  x[12] = 0; x[13] = 0; x[14] = 0; x[15] = 1;
}

template <class T>
inline void Transform<T>::setTranslation(const Vec3<T>& t) {
  x[3] = t(0); x[7] = t(1); x[11] = t(2);
  x[12] = 0; x[13] = 0; x[14] = 0; x[15] = 1;
}

template <class T>
inline void Transform<T>::set(const Rotation<T>& r, const Vec3<T>& t) {
  setRotation(r);
  setTranslation(t);
}

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

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

template <class T>
inline void Transform<T>::rotateZ(T theta)
{
  Transform<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 Transform<T>::translate(T _x, T _y, T _z)
{
  x[3] += _x;
  x[7] += _y;
  x[11] += _z;
}

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

//template <class T>
//inline Vec<T,4> Transform<T>::operator * (const Vec3<T>& v) const{
//  return (*(Mat<T,4,4>*)this)*Vec4<T>(v(0),v(1),v(2),T(1.0));
//}

template <class T>
inline Vec3<T>& Transform<T>::apply(Vec3<T>& v) const {
  Vec4<T> v0(v[0],v[1],v[2],T(1));
  Vec<T,4> v1 = (*this)*v0;
  v.set(v1[0],v1[1],v1[2]);
  return v;
}

template <class T>
inline Vec3<T> Transform<T>::apply(const Vec3<T>& v) const {
  Vec4<T> v0(v[0],v[1],v[2],T(1));
  Vec<T,4> v1 = (*this)*v0;
  return(Vec3<T>(v1[0],v1[1],v1[2]));
}

template <class T>
inline Transform<T> Transform<T>::inverted() const{
  Rotation<T> r(x[0],x[4],x[8],
    x[1],x[5],x[9],
    x[2],x[6],x[10]);
  Vec3<T> t(-x[3],-x[7],-x[11]);
  return Transform<T>(r,r*t);
}

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

template <class T>
inline bool Transform<T>::verifyTag(std::string got, std::string expected) {
    if (got == expected){
      return true;
    }
    else{
      std::cerr << "ERROR: input stream format error." << std::endl;
      std::cerr << "Expected: " << expected << " Got: " << got << std::endl;
      return false;
    }
 }

template <class T>
std::ostream& operator<<(std::ostream& os, const Transform<T>& trans){
  Rotation<T> rot;
  Vec3<T> t;
  trans.get( rot,  t);
  EulerAngles<T> r;
  r.set(rot);

  os << "rot (Euler Angles) " << r << std::endl;
  os << "trans " << t << std::endl;

  return os;
}


template <class T>
std::istream& operator>>(std::istream& is, Transform<T>& trans){
  using namespace std;
  Rotation<T> rot;
  Vec3<T> t;
  string buf;

  is >> buf;

  //old format (rot as quat)
  if (trans.verifyTag(buf,"r")){
    Quaternion<T> q;
    if (trans.verifyTag(buf,"r")){
      is >> q;
    }
    else{
      cout << "format error" << endl;
      exit(0);
    }

    is >> buf;
    if (trans.verifyTag(buf,"t")){
      is >> t;
    }
    else{
      cout << "format error" << endl;
      exit(0);
    }

    rot = Rotation<T>(q);

  }
  //new format (rot as euler angles)
  else{
     EulerAngles<T> ea;
     if (trans.verifyTag(buf,"rot")){
      is >> ea;
    }
    else{
      cout << "format error" << endl;
      exit(0);
    }

    is >> buf;
    if (trans.verifyTag(buf,"trans")){
      is >> t;
    }
    else{
      cout << "format error" << endl;
      exit(0);
    }
     rot = Rotation<T>(ea);
  }

  trans.set(rot,t);

  return is;
}

//==============================================================================
} // namespace rtc
//==============================================================================
#endif // RTC_TRANSFORM_H defined
//==============================================================================