rtcRotation.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 .......: rtcRotation.h
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 08/16/2006
 * modified ...: $Date: 2009-01-21 18:19:16 -0800 (Wed, 21 Jan 2009) $
 * changed by .: $Author: benjaminpitzer $
 * revision ...: $Revision: 14 $
 */
#ifndef RTC_ROTATION_H
#define RTC_ROTATION_H

//== INCLUDES ==================================================================
#include "rtc/rtcMath.h"
#include "rtc/rtcEulerAngles.h"
#include "rtc/rtcQuaternion.h"
#include "rtc/rtcSMat3.h"

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

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 Rotation: public SMat3<T> {
public:
  Rotation();
  Rotation(const T x11, const T x12, const T x13,
           const T x21, const T x22, const T x23,
           const T x31, const T x32, const T x33);
  Rotation(const Mat<T,3,3>& m);
  Rotation(const Quaternion<T>& q);
  Rotation(const EulerAngles<T>& e);

  template <class U> Rotation(const Mat<U,3,3>& m);

  void set(const Quaternion<T>& q);
  void set(const EulerAngles<T>& e);
  void set(const T theta, const T phi);

  void apply(Vec3<T>& v) const;
  Vec3<T> apply(const Vec3<T>& v) const;

  using SMat3<T>::x;
  using SMat3<T>::set;
};

// Declare a few common typdefs
typedef Rotation<float> Rotationf;
typedef Rotation<double> Rotationd;

//==============================================================================
// Rotation<T>
//==============================================================================

// Constructors

template <class T>
inline Rotation<T>::Rotation() {
  x[0] = x[4] = x[8] = T(1);
  x[1] = x[2] = x[3] = x[5] = x[6] = x[7] = 0;
}

template <class T>
inline Rotation<T>::Rotation(const T x11, const T x12, const T x13,
                             const T x21, const T x22, const T x23,
                             const T x31, const T x32, const T x33) {
  x[0] = x11; x[1] = x12; x[2] = x13;
  x[3] = x21; x[4] = x22; x[5] = x23;
  x[6] = x31; x[7] = x32; x[8] = x33;
}

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

template <class T>
inline Rotation<T>::Rotation(const Quaternion<T>& q) {
  set(q);
}

template <class T>
inline Rotation<T>::Rotation(const EulerAngles<T>& e) {
  set(e);
}

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

// Mutators

template <class T>
inline void Rotation<T>::set(const Quaternion<T>& q) {
  x[0] = q.x[0]*q.x[0] + q.x[1]*q.x[1] - q.x[2]*q.x[2] - q.x[3]*q.x[3];
  x[1] = T(2)*(q.x[1]*q.x[2] - q.x[0]*q.x[3]);
  x[2] = T(2)*(q.x[1]*q.x[3] + q.x[0]*q.x[2]);
  x[3] = T(2)*(q.x[1]*q.x[2] + q.x[0]*q.x[3]);
  x[4] = q.x[0]*q.x[0] - q.x[1]*q.x[1] + q.x[2]*q.x[2] - q.x[3]*q.x[3];
  x[5] = T(2)*(q.x[2]*q.x[3] - q.x[0]*q.x[1]);
  x[6] = T(2)*(q.x[1]*q.x[3] - q.x[0]*q.x[2]);
  x[7] = T(2)*(q.x[2]*q.x[3] + q.x[0]*q.x[1]);
  x[8] = q.x[0]*q.x[0] - q.x[1]*q.x[1] - q.x[2]*q.x[2] + q.x[3]*q.x[3];
}

template <class T>
inline void Rotation<T>::set(const EulerAngles<T>& e) {
  T roll = e.x[0]; T pitch = e.x[1]; T yaw = e.x[2];
  x[0] = cos(yaw)*cos(pitch);
  x[1] = cos(yaw)*sin(pitch)*sin(roll) - sin(yaw)*cos(roll);
  x[2] = cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll);

  x[3] = sin(yaw)*cos(pitch);
  x[4] = sin(yaw)*sin(pitch)*sin(roll) + cos(yaw)*cos(roll);
  x[5] = sin(yaw)*sin(pitch)*cos(roll) - cos(yaw)*sin(roll);

  x[6] = -sin(pitch);
  x[7] = cos(pitch)*sin(roll);
  x[8] = cos(pitch)*cos(roll);
}

template <class T>
inline void Rotation<T>::set(const T theta, const T phi) {
  Quaternion<T> q(Vec3f(cos(theta)*sin(phi),sin(theta)*sin(phi),cos(phi)).cross(Vec3f(1,0,0)),acos(cos(theta)*sin(phi)));
  set(q);
}

template <class T>
inline void Rotation<T>::apply(Vec3<T>& v) const {
  v.set((*this)*v);
}

template <class T>
inline Vec3<T> Rotation<T>::apply(const Vec3<T>& v) const {
  return((*this)*v);
}

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