rtcEulerAngles.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 .......: rtcEulerAngles.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_EULERANGLES_H
#define RTC_EULERANGLES_H

//== INCLUDES ==================================================================
#include "rtc/rtcMath.h"
#include "rtc/rtcRotation.h"
#include "rtc/rtcTransform.h"
#include "rtc/rtcQuaternion.h"
#include "rtc/rtcVec3.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 EulerAngles: public Vec3<T> {
public:
  EulerAngles();
  EulerAngles(const T roll, const T pitch, const T yaw);
  EulerAngles(const Vec<T,3>& v);
  EulerAngles(const Quaternion<T>& q);
  EulerAngles(const Rotation<T>& r);

  void set(const Vec<T,3>& v);
  void set(const T roll, const T pitch, const T yaw);
  void set(const Quaternion<T>& q);
  void set(const Rotation<T>& r);
  void bound();

  T yaw() const;
  T pitch() const;
  T roll() const;
  Vec3<T> toDegrees();

  using Vec3<T>::x;
};

// Declare a few common typdefs
typedef EulerAngles<float> EulerAnglesf;
typedef EulerAngles<double> EulerAnglesd;

//==============================================================================
// EulerAngles<T>
//==============================================================================

template <class T>
inline EulerAngles<T>::EulerAngles() {
  set(T(0),T(0),T(0));
}

template <class T>
inline EulerAngles<T>::EulerAngles(const T roll,
           const T pitch,
           const T yaw) {
  set(roll,pitch,yaw);
}

template <class T>
inline EulerAngles<T>::EulerAngles(const Vec<T,3>& v) : Vec3<T>(v) {
  bound();
}

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

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

// Mutators

template <class T>
inline void EulerAngles<T>::set(const Vec<T,3>& v) {
  Vec<T,3>::set(v);
  bound();
}

template <class T>
inline void EulerAngles<T>::set(const T roll, const T pitch, const T yaw) {
  x[0] = roll; x[1] = pitch; x[2] = yaw;
  bound();
}

template <class T>
inline void EulerAngles<T>::set(const Quaternion<T>& q) {
  set(atan2(T(2)*(q.x[2]*q.x[3] + q.x[0]*q.x[1]),q.x[0]*q.x[0] - q.x[1]*q.x[1] - q.x[2]*q.x[2] + q.x[3]*q.x[3]),
      asin(T(-2)*(q.x[1]*q.x[3] - q.x[0]*q.x[2])),
      atan2(T(2)*(q.x[1]*q.x[2] + q.x[0]*q.x[3]),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 EulerAngles<T>::set(const Rotation<T>& r) {
  set((T)atan2(double(r.x[7]),double(r.x[8])),
      (T)asin(-double(r.x[6])),
      (T)atan2(double(r.x[3]),double(r.x[0])));
}

template <class T>
inline void EulerAngles<T>::bound() {
  for (int i=0;i<3;i++) {
    x[i] = fmod(x[i],T(TWOPI));
    if (x[i] > T(PI)) x[i] -= T(TWOPI);
    if (x[i] < -T(PI)) x[i] += T(TWOPI);
  }
  if (rtc_abs(x[1]) > T(PI_2)) {
    x[0] -= rtc_sign(x[0])*T(PI);
    x[1] = rtc_sign(x[1])*(T(PI) - rtc_abs(x[1]));
    x[2] -= rtc_sign(x[2])*T(PI);
  }
}

template <class T>
inline T EulerAngles<T>::roll() const { return x[0]; }

template <class T>
inline T EulerAngles<T>::pitch() const { return x[1]; }

template <class T>
inline T EulerAngles<T>::yaw() const { return x[2]; }

template <class T>
inline Vec3<T> EulerAngles<T>::toDegrees() {
  return Vec3<T>((T)R2D*x[0],(T)R2D*x[1],(T)R2D*x[2]);
}

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