alpose2d.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_ALPOSE2D_H_
#define _LIBALMATH_ALMATH_TYPES_ALPOSE2D_H_

#include <vector>
#include <cmath>

namespace AL {
  namespace Math {

    struct Pose2D {
      float x;
      float y;
      float theta;


      Pose2D();


      explicit Pose2D(float pInit);


      explicit Pose2D(
        float pX,
        float pY,
        float pTheta);


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

      static Pose2D fromPolarCoordinates(const float pRadius, const float pAngle);

      inline Pose2D operator+ (const Pose2D& pPos2) const
      {
        return Pose2D(x + pPos2.x, y + pPos2.y, theta + pPos2.theta);
      }

      inline Pose2D operator- (const Pose2D& pPos2) const
      {
        return Pose2D(x - pPos2.x, y - pPos2.y, theta - pPos2.theta);
      }

      inline Pose2D operator+ (void) const
      {
        return *this;
      }

      inline Pose2D operator- () const
      {
        return Pose2D(-x, -y, -theta);
      }

      Pose2D& operator+= (const Pose2D& pPos2);

      Pose2D& operator-= (const Pose2D& pPos2);

      Pose2D& operator*= (const Pose2D& pPos2);

      inline Pose2D operator* (const Pose2D& pPos2) const
      {
        return Pose2D(
              x + std::cos(theta) * pPos2.x - std::sin(theta) * pPos2.y,
              y + std::sin(theta) * pPos2.x + std::cos(theta) * pPos2.y,
              theta + pPos2.theta);
      }

      bool operator==(const Pose2D& pPos2) const;

      bool operator!=(const Pose2D& pPos2) const;

      inline Pose2D operator* (float pVal) const
      {
        return Pose2D(x*pVal, y*pVal, theta*pVal);
      }

      Pose2D operator/ (float pVal) const;

      Pose2D& operator*= (float pVal);

      Pose2D& operator/= (float pVal);

      float distanceSquared(const Pose2D& pPos2) const;

      float distance(const Pose2D& pPos2) const;

      Pose2D inverse() const;

      Pose2D diff(const Pose2D& pPos2) const;

      bool isNear(const Pose2D& pPos2,
                  const float&  pEpsilon=0.0001f) const;

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

      inline float norm() const
      {
        return std::sqrt(x * x + y * y);
      }

      Pose2D normalize() const;

      inline float getAngle(void) const
      {
        return std::atan2(y, x);
      }
    }; // end struct


    float distanceSquared(
      const Pose2D& pPos1,
      const Pose2D& pPos2);

    float distance(
      const Pose2D& pPos1,
      const Pose2D& pPos2);

    void pose2dInvertInPlace(Pose2D& pPos);

    Pose2D pinv(const Pose2D& pPos);


    Pose2D pose2dDiff(
      const Pose2D& pPos1,
      const Pose2D& pPos2);

    Pose2D pose2DInverse(const Pose2D& pPos);

    void pose2DInverse(
      const Pose2D& pPos,
      Pose2D&       pRes);


  } // end namespace math
} // end namespace AL
#endif  // _LIBALMATH_ALMATH_TYPES_ALPOSE2D_H_