rotation.h

Go to the documentation of this file.

#ifndef ION_MATH_ROTATION_H_
#define ION_MATH_ROTATION_H_

#include <istream>  // NOLINT
#include <ostream>  // NOLINT
#include <string>

#include "ion/base/stringutils.h"
#include "ion/math/angle.h"
#include "ion/math/matrix.h"
#include "ion/math/vector.h"
#include "ion/math/vectorutils.h"

namespace ion {
namespace math {

template <typename T>
class Rotation {
 public:
  typedef Angle<T> AngleType;
  typedef Vector<3, T> VectorType;
  typedef Vector<4, T> QuaternionType;

  Rotation() {
    quat_.Set(static_cast<T>(0), static_cast<T>(0), static_cast<T>(0),
              static_cast<T>(1));
  }

  static Rotation Identity() {
    return Rotation();
  }

  bool IsIdentity() const {
    return quat_[3] == static_cast<T>(1) ||
           quat_[3] == static_cast<T>(-1);
  }

  void SetQuaternion(const QuaternionType& quaternion) {
    quat_ = Normalized(quaternion);
  }

  const QuaternionType& GetQuaternion() const { return quat_; }

  void SetAxisAndAngle(const VectorType& axis, const AngleType& angle);

  void GetAxisAndAngle(VectorType* axis, AngleType* angle) const;

  void GetEulerAngles(AngleType* yaw, AngleType* pitch, AngleType* roll) const;

  static Rotation FromAxisAndAngle(const VectorType& axis,
                                   const AngleType& angle) {
    Rotation r;
    r.SetAxisAndAngle(axis, angle);
    return r;
  }

  static Rotation FromQuaternion(const QuaternionType& quat) {
    Rotation r;
    r.SetQuaternion(quat);
    return r;
  }

  static Rotation FromRotationMatrix(const Matrix<3, T>& mat);

  static Rotation RotateInto(const VectorType& from,
                             const VectorType& to);

  friend Rotation operator-(const Rotation& r) {
    return Rotation(-r.quat_[0], -r.quat_[1], -r.quat_[2], r.quat_[3]);
  }

  Rotation& operator*=(const Rotation& r) {
    const QuaternionType &qr = r.quat_;
    QuaternionType &qt = quat_;
    SetQuaternion(QuaternionType(
        qr[3] * qt[0] + qr[0] * qt[3] + qr[2] * qt[1] - qr[1] * qt[2],
        qr[3] * qt[1] + qr[1] * qt[3] + qr[0] * qt[2] - qr[2] * qt[0],
        qr[3] * qt[2] + qr[2] * qt[3] + qr[1] * qt[0] - qr[0] * qt[1],
        qr[3] * qt[3] - qr[0] * qt[0] - qr[1] * qt[1] - qr[2] * qt[2]));
    return *this;
  }

  friend const Rotation operator*(const Rotation& r0, const Rotation& r1) {
    Rotation r = r0;
    r *= r1;
    return r;
  }

  const VectorType operator*(const VectorType& v) const;
  const Point<3, T> operator*(const Point<3, T>& p) const;

  friend bool operator==(const Rotation& v0, const Rotation& v1) {
    return v0.quat_ == v1.quat_ || v0.quat_ == -v1.quat_;
  }
  friend bool operator!=(const Rotation& v0, const Rotation& v1) {
    return v0.quat_ != v1.quat_ && v0.quat_ != -v1.quat_;
  }

  static Rotation Slerp(const Rotation& r0, const Rotation& r1, T t);

 private:
  Rotation(T q0, T q1, T q2, T q3) : quat_(q0, q1, q2, q3) {}

  VectorType ApplyToVector(const VectorType& v) const {
    VectorType im(quat_[0], quat_[1], quat_[2]);
    VectorType temp = static_cast<T>(2) * Cross(im, v);
    return v + quat_[3] * temp + Cross(im, temp);
  }

  QuaternionType quat_;
};

template <typename T>
const typename Rotation<T>::VectorType Rotation<T>::operator*(
    const typename Rotation<T>::VectorType& v) const {
  return ApplyToVector(v);
}

template <typename T>
const Point<3, T> Rotation<T>::operator*(const Point<3, T>& p) const {
  return ApplyToVector(p - Point<3, T>::Zero()) + Point<3, T>::Zero();
}

template <typename T>
std::ostream& operator<<(std::ostream& out, const Rotation<T>& a) {
  Vector<3, T> axis;
  Angle<T> angle;
  a.GetAxisAndAngle(&axis, &angle);
  return out << "ROT[" << axis << ": " << angle << "]";
}

template <typename T>
std::istream& operator>>(std::istream& in, Rotation<T>& a) {
  if (base::GetExpectedString(in, "ROT[")) {
    Vector<3, T> axis;
    Angle<T> angle;
    if (in >> axis >> base::GetExpectedChar<':'> >> angle
           >> base::GetExpectedChar<']'>)
      a.SetAxisAndAngle(axis, angle);
  }

  return in;
}




typedef Rotation<float> Rotationf;
typedef Rotation<double> Rotationd;

}  // namespace math
}  // namespace ion

#endif  // ION_MATH_ROTATION_H_