alquaternion.h

/*
 * Copyright (c) 2012 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */


#pragma once
#ifndef _LIBALMATH_ALMATH_TYPES_ALQUATERNION_H_
#define _LIBALMATH_ALMATH_TYPES_ALQUATERNION_H_

#include <vector>

namespace AL {
  namespace Math {

    struct Quaternion {
      float w;
      float x;
      float y;
      float z;


      Quaternion();


      Quaternion(
        float pW,
        float pX,
        float pY,
        float pZ);


      Quaternion(const std::vector<float>& pFloats);

      Quaternion& operator*= (const Quaternion& pQua2);

      inline Quaternion operator* (const Quaternion& pQua2) const
      {
        return Quaternion(
              w*pQua2.w - x*pQua2.x - y*pQua2.y - z*pQua2.z,
              w*pQua2.x + pQua2.w*x + y*pQua2.z - z*pQua2.y,
              w*pQua2.y + pQua2.w*y + z*pQua2.x - x*pQua2.z,
              w*pQua2.z + pQua2.w*z + x*pQua2.y - y*pQua2.x);
      }

      bool operator== (const Quaternion& pQua2) const;

      bool operator!= (const Quaternion& pQua2) const;

      inline Quaternion operator* (float pVal) const
      {
        return Quaternion(w*pVal, x*pVal, y*pVal, z*pVal);
      }

      Quaternion operator/ (float pVal) const;

      Quaternion& operator*= (float pVal);

      Quaternion& operator/= (float pVal);

      bool isNear(
        const Quaternion& pQua2,
        const float&      pEpsilon=0.0001f) const;

      float norm() const;

      Quaternion normalize() const;

      Quaternion inverse() const;

      static Quaternion fromAngleAndAxisRotation(
        const float pAngle,
        const float pAxisX,
        const float pAxisY,
        const float pAxisZ);

      void toVector(std::vector<float>& pReturnVector) const;
      std::vector<float> toVector(void) const;
    };

    float norm(const Quaternion& pQua);

    Quaternion normalize(const Quaternion& pQua);


    void quaternionInverse(
      const Quaternion& pQua,
      Quaternion&       pQuaOut);

    Quaternion quaternionInverse(const Quaternion& pQua);


    Quaternion quaternionFromAngleAndAxisRotation(
      const float pAngle,
      const float pAxisX,
      const float pAxisY,
      const float pAxisZ);


    void angleAndAxisRotationFromQuaternion(
      const Quaternion& pQuaternion,
      float& pAngle,
      float& pAxisX,
      float& pAxisY,
      float& pAxisZ);

    std::vector<float> angleAndAxisRotationFromQuaternion(
      const Quaternion& pQuaternion);


  } // end namespace math
} // end namespace al
#endif  // _LIBALMATH_ALMATH_TYPES_ALQUATERNION_H_