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& pQu2);

      Quaternion operator* (const Quaternion& pQua2) const;

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

      bool operator!= (const Quaternion& pQua2) 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);

      std::vector<float> toVector() 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);

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