alrotation.h

Go to the documentation of this file.

/*
 * 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;

      void normalizeRotation(void);

      bool isRotation(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 fromRotXPi() {
        Rotation r; // identity
        r.r2_c2 = -1.f;
        r.r3_c3 = -1.f;
        return r;
      }


      static Rotation fromRotY(const float pRotY);

      static Rotation fromRotYPi() {
        Rotation r; // identity
        r.r1_c1 = -1.f;
        r.r3_c3 = -1.f;
        return r;
      }


      static Rotation fromRotZ(const float pRotZ);

      static Rotation fromRotZPi() {
        Rotation r; // identity
        r.r1_c1 = -1.f;
        r.r2_c2 = -1.f;
        return r;
      }

      static Rotation fromRotZHalfPi() {
        Rotation r; // identity
        r.r1_c1 = 0.f;
        r.r1_c2 = -1.f;
        r.r2_c1 = 1.f;
        r.r2_c2 = 0.f;
        return r;
      }

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


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

    }; // end struct

    Rotation transpose(const Rotation& pRot);



    float determinant(const Rotation& pRot);

    void normalizeRotation(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_