alposition6d.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_ALPOSITION6D_H_
#define _LIBALMATH_ALMATH_TYPES_ALPOSITION6D_H_

#include <vector>

namespace AL {
  namespace Math {

    struct Position6D {
      float x;
      float y;
      float z;
      float wx;
      float wy;
      float wz;


      Position6D();


      explicit Position6D(float pInit);


      Position6D(
        float pX,
        float pY,
        float pZ,
        float pWx,
        float pWy,
        float pWz);


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

      inline Position6D operator+ (const Position6D& pPos2) const
      {
        return Position6D(
              x + pPos2.x,
              y + pPos2.y,
              z + pPos2.z,
              wx + pPos2.wx,
              wy + pPos2.wy,
              wz + pPos2.wz);
      }

      inline Position6D operator- (const Position6D& pPos2) const
      {
        return Position6D(
              x - pPos2.x,
              y - pPos2.y,
              z - pPos2.z,
              wx - pPos2.wx,
              wy - pPos2.wy,
              wz - pPos2.wz);
      }

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

      inline Position6D operator- () const
      {
        return Position6D(-x, -y, -z, -wx, -wy, -wz);
      }

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

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

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

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

      inline Position6D operator* (float pVal) const
      {
        return Position6D(
              x * pVal,
              y * pVal,
              z * pVal,
              wx * pVal,
              wy * pVal,
              wz * pVal);
      }

      Position6D operator/ (float pVal) const;

      Position6D& operator*= (float pVal);

      Position6D& operator/= (float pVal);

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

      float distanceSquared(const Position6D& pPos2) const;

      float distance(const Position6D& pPos2) const;

      float norm() const;

      void toVector (std::vector<float>& pReturnVector) const;
      std::vector<float> toVector (void) const;
    }; // end struct


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


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

    float norm(const Position6D& pPos);

    Position6D normalize(const Position6D& pPos);

  } // end namespace math
} // end namespace al

#endif  // _LIBALMATH_ALMATH_TYPES_ALPOSITION6D_H_