alvelocity6d.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_ALVELOCITY6D_H_
#define _LIBALMATH_ALMATH_TYPES_ALVELOCITY6D_H_

#include <vector>

namespace AL {
namespace Math {


struct Velocity6D {
  float xd;
  float yd;
  float zd;
  float wxd;
  float wyd;
  float wzd;


  Velocity6D();


  explicit Velocity6D(float pInit);


  Velocity6D(
    float pXd,
    float pYd,
    float pZd,
    float pWxd,
    float pWyd,
    float pWzd);


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

  inline Velocity6D operator+ (const Velocity6D& pVel2) const
  {
    return Velocity6D(
          xd  + pVel2.xd,
          yd  + pVel2.yd,
          zd  + pVel2.zd,
          wxd + pVel2.wxd,
          wyd + pVel2.wyd,
          wzd + pVel2.wzd);
  }

  inline Velocity6D operator- (const Velocity6D& pVel2) const
  {
    return Velocity6D(
          xd  - pVel2.xd,
          yd  - pVel2.yd,
          zd  - pVel2.zd,
          wxd - pVel2.wxd,
          wyd - pVel2.wyd,
          wzd - pVel2.wzd);
  }

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

  inline Velocity6D operator- () const
  {
    return Velocity6D(-xd, -yd, -zd, -wxd, -wyd, -wzd);
  }

  inline Velocity6D operator* (const float pVal) const
  {
    return Velocity6D(
          xd*pVal,
          yd*pVal,
          zd*pVal,
          wxd*pVal,
          wyd*pVal,
          wzd*pVal);
  }

  Velocity6D operator/ (const float pVal) const;

  bool operator== (const Velocity6D& pVel2) const;

  bool operator!= (const Velocity6D& pVel2) const;

  Velocity6D& operator*= (const float pVal);

  Velocity6D& operator/= (const float pVal);

  bool isNear(
    const Velocity6D& pVel2,
    const float&      pEpsilon=0.0001f) const;

  float norm() const;

  Velocity6D normalize() const;

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

Velocity6D operator* (
  const float       pVal,
  const Velocity6D& pVel);

float norm(const Velocity6D& pVel);

Velocity6D normalize(const Velocity6D& pVel);

} // end namespace Math
} // end namespace AL
#endif  // _LIBALMATH_ALMATH_TYPES_ALVELOCITY6D_H_