almath/types/alrotation.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_ALROTATION_H_
#define _LIBALMATH_ALMATH_TYPES_ALROTATION_H_

#include <vector>

namespace AL {
  namespace Math {





    struct Rotation
    {
      float r1_c1;
      float r1_c2;
      float r1_c3;
      float r2_c1;
      float r2_c2;
      float r2_c3;
      float r3_c1;
      float r3_c2;
      float r3_c3;



      Rotation();







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

      Rotation& operator*= (const Rotation& pRot2);

      Rotation operator* (const Rotation& pRot2) const;

      bool operator==(const Rotation& pRot2) const;

      bool operator!=(const Rotation& pRot2) const;

      bool isNear(
        const Rotation& pRot2,
        const float&    pEpsilon=0.0001f) const;

      Rotation transpose() const;






      float determinant() const;

      static Rotation fromQuaternion(
        const float pA,
        const float pB,
        const float pC,
        const float pD);


      static Rotation fromAngleDirection(
        const float pAngle,
        const float pX,
        const float pY,
        const float pZ);




      static Rotation fromRotX(const float pRotX);




      static Rotation fromRotY(const float pRotY);




      static Rotation fromRotZ(const float pRotZ);

      static Rotation from3DRotation(
        const float& pWX,
        const float& pWY,
        const float& pWZ);



      std::vector<float> toVector() const;

    }; // end struct

    Rotation transpose(const Rotation& pRot);









    float determinant(const Rotation& pRot);


    Rotation rotationFromQuaternion(
      const float pA,
      const float pB,
      const float pC,
      const float pD);

    Rotation rotationFromAngleDirection(
      const float pAngle,
      const float pX,
      const float pY,
      const float pZ);

    void applyRotation(
      const AL::Math::Rotation& pRot,
      float&                    pX,
      float&                    pY,
      float&                    pZ);








    Rotation rotationFromRotX(const float pRotX);








    Rotation rotationFromRotY(const float pRotY);








    Rotation rotationFromRotZ(const float pRotZ);

    Rotation rotationFrom3DRotation(
      const float& pWX,
      const float& pWY,
      const float& pWZ);

  }
}
#endif  // _LIBALMATH_ALMATH_TYPES_ALROTATION_H_