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>

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

      Pose2D operator+ (const Pose2D& pPos2) const;

      Pose2D operator- (const Pose2D& pPos2) const;

      Pose2D operator+ () const;

      Pose2D operator- () const;

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

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

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

      Pose2D operator* (const Pose2D& pPos2) const;

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

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

      Pose2D operator* (float pVal) const;

      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;

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

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

    }; // end struct


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

    float distance(
      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_