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

      Position6D operator+ (const Position6D& pPos2) const;

      Position6D operator- (const Position6D& pPos2) const;

      Position6D operator+ () const;

      Position6D operator- () const;

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

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

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

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

      Position6D operator* (float pVal) const;

      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;

      std::vector<float> toVector () 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_