Vector2.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef IGNITION_MATH_VECTOR2_HH_
#define IGNITION_MATH_VECTOR2_HH_

#include <algorithm>
#include <cmath>
#include <limits>

#include <ignition/math/Helpers.hh>
#include <ignition/math/config.hh>

namespace ignition
{
  namespace math
  {
    // Inline bracket to help doxygen filtering.
    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
    //
    template<typename T>
    class Vector2
    {
      public: static const Vector2<T> Zero;

      public: static const Vector2<T> One;

      public: static const Vector2 NaN;

      public: Vector2()
      {
        this->data[0] = 0;
        this->data[1] = 0;
      }

      public: Vector2(const T &_x, const T &_y)
      {
        this->data[0] = _x;
        this->data[1] = _y;
      }

      public: Vector2(const Vector2<T> &_v)
      {
        this->data[0] = _v[0];
        this->data[1] = _v[1];
      }

      public: virtual ~Vector2() {}

      public: T Sum() const
      {
        return this->data[0] + this->data[1];
      }

      public: double Distance(const Vector2 &_pt) const
      {
        return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
                    (this->data[1]-_pt[1])*(this->data[1]-_pt[1]));
      }

      public: T Length() const
      {
        return static_cast<T>(sqrt(this->SquaredLength()));
      }

      public: T SquaredLength() const
      {
        return
          this->data[0] * this->data[0] +
          this->data[1] * this->data[1];
      }

      public: void Normalize()
      {
        T d = this->Length();

        if (!equal<T>(d, static_cast<T>(0.0)))
        {
          this->data[0] /= d;
          this->data[1] /= d;
        }
      }

      public: Vector2 Normalized() const
      {
        Vector2<T> result = *this;
        result.Normalize();
        return result;
      }

      public: Vector2 Round()
      {
        this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
        this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
        return *this;
      }

      public: Vector2 Rounded() const
      {
        Vector2<T> result = *this;
        result.Round();
        return result;
      }

      public: void Set(T _x, T _y)
      {
        this->data[0] = _x;
        this->data[1] = _y;
      }

      public: T Dot(const Vector2<T> &_v) const
      {
        return (this->data[0] * _v[0]) + (this->data[1] * _v[1]);
      }

      public: Vector2 Abs() const
      {
        return Vector2(std::abs(this->data[0]),
                       std::abs(this->data[1]));
      }

      public: T AbsDot(const Vector2<T> &_v) const
      {
        return std::abs(this->data[0] * _v[0]) +
               std::abs(this->data[1] * _v[1]);
      }

      public: inline void Correct()
      {
        // std::isfinite works with floating point values,
        // need to explicit cast to avoid ambiguity in vc++.
        if (!std::isfinite(static_cast<double>(this->data[0])))
          this->data[0] = 0;
        if (!std::isfinite(static_cast<double>(this->data[1])))
          this->data[1] = 0;
      }

      public: void Max(const Vector2<T> &_v)
      {
        this->data[0] = std::max(_v[0], this->data[0]);
        this->data[1] = std::max(_v[1], this->data[1]);
      }

      public: void Min(const Vector2<T> &_v)
      {
        this->data[0] = std::min(_v[0], this->data[0]);
        this->data[1] = std::min(_v[1], this->data[1]);
      }

      public: T Max() const
      {
        return std::max(this->data[0], this->data[1]);
      }

      public: T Min() const
      {
        return std::min(this->data[0], this->data[1]);
      }

      public: Vector2 &operator=(const Vector2 &_v)
      {
        this->data[0] = _v[0];
        this->data[1] = _v[1];

        return *this;
      }

      public: const Vector2 &operator=(T _v)
      {
        this->data[0] = _v;
        this->data[1] = _v;

        return *this;
      }

      public: Vector2 operator+(const Vector2 &_v) const
      {
        return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]);
      }

      // \return this
      public: const Vector2 &operator+=(const Vector2 &_v)
      {
        this->data[0] += _v[0];
        this->data[1] += _v[1];

        return *this;
      }

      public: inline Vector2<T> operator+(const T _s) const
      {
        return Vector2<T>(this->data[0] + _s,
                          this->data[1] + _s);
      }

      public: friend inline Vector2<T> operator+(const T _s,
                                                 const Vector2<T> &_v)
      {
        return _v + _s;
      }

      public: const Vector2<T> &operator+=(const T _s)
      {
        this->data[0] += _s;
        this->data[1] += _s;

        return *this;
      }

      public: inline Vector2 operator-() const
      {
        return Vector2(-this->data[0], -this->data[1]);
      }

      public: Vector2 operator-(const Vector2 &_v) const
      {
        return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]);
      }

      public: const Vector2 &operator-=(const Vector2 &_v)
      {
        this->data[0] -= _v[0];
        this->data[1] -= _v[1];

        return *this;
      }

      public: inline Vector2<T> operator-(const T _s) const
      {
        return Vector2<T>(this->data[0] - _s,
                          this->data[1] - _s);
      }

      public: friend inline Vector2<T> operator-(const T _s,
                                                 const Vector2<T> &_v)
      {
        return {_s - _v.X(), _s - _v.Y()};
      }

      public: const Vector2<T> &operator-=(T _s)
      {
        this->data[0] -= _s;
        this->data[1] -= _s;

        return *this;
      }

      public: const Vector2 operator/(const Vector2 &_v) const
      {
        return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]);
      }

      public: const Vector2 &operator/=(const Vector2 &_v)
      {
        this->data[0] /= _v[0];
        this->data[1] /= _v[1];

        return *this;
      }

      public: const Vector2 operator/(T _v) const
      {
        return Vector2(this->data[0] / _v, this->data[1] / _v);
      }

      public: const Vector2 &operator/=(T _v)
      {
        this->data[0] /= _v;
        this->data[1] /= _v;

        return *this;
      }

      public: const Vector2 operator*(const Vector2 &_v) const
      {
        return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]);
      }

      public: const Vector2 &operator*=(const Vector2 &_v)
      {
        this->data[0] *= _v[0];
        this->data[1] *= _v[1];

        return *this;
      }

      public: const Vector2 operator*(T _v) const
      {
        return Vector2(this->data[0] * _v, this->data[1] * _v);
      }

      public: friend inline const Vector2 operator*(const T _s,
                                                    const Vector2 &_v)
      {
        return Vector2(_v * _s);
      }

      public: const Vector2 &operator*=(T _v)
      {
        this->data[0] *= _v;
        this->data[1] *= _v;

        return *this;
      }

      public: bool Equal(const Vector2 &_v, const T &_tol) const
      {
        return equal<T>(this->data[0], _v[0], _tol)
            && equal<T>(this->data[1], _v[1], _tol);
      }

      public: bool operator==(const Vector2 &_v) const
      {
        return this->Equal(_v, static_cast<T>(1e-6));
      }

      public: bool operator!=(const Vector2 &_v) const
      {
        return !(*this == _v);
      }

      public: bool IsFinite() const
      {
        // std::isfinite works with floating point values,
        // need to explicit cast to avoid ambiguity in vc++.
        return std::isfinite(static_cast<double>(this->data[0])) &&
               std::isfinite(static_cast<double>(this->data[1]));
      }

      public: T &operator[](const std::size_t _index)
      {
        return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
      }

      public: T operator[](const std::size_t _index) const
      {
        return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
      }

      public: inline T X() const
      {
        return this->data[0];
      }

      public: inline T Y() const
      {
        return this->data[1];
      }

      public: inline T &X()
      {
        return this->data[0];
      }

      public: inline T &Y()
      {
        return this->data[1];
      }

      public: inline void X(const T &_v)
      {
        this->data[0] = _v;
      }

      public: inline void Y(const T &_v)
      {
        this->data[1] = _v;
      }

      public: friend std::ostream
      &operator<<(std::ostream &_out, const Vector2<T> &_pt)
      {
        _out << _pt[0] << " " << _pt[1];
        return _out;
      }

      public: bool operator<(const Vector2<T> &_pt) const
      {
        return this->data[0] < _pt[0] || this->data[1] < _pt[1];
      }

      public: friend std::istream
      &operator>>(std::istream &_in, Vector2<T> &_pt)
      {
        T x, y;
        // Skip white spaces
        _in.setf(std::ios_base::skipws);
        _in >> x >> y;
        if (!_in.fail())
        {
          _pt.Set(x, y);
        }
        return _in;
      }

      private: T data[2];
    };

    template<typename T>
    const Vector2<T> Vector2<T>::Zero(0, 0);

    template<typename T>
    const Vector2<T> Vector2<T>::One(1, 1);

    template<typename T>
    const Vector2<T> Vector2<T>::NaN(
        std::numeric_limits<T>::quiet_NaN(),
        std::numeric_limits<T>::quiet_NaN());

    typedef Vector2<int> Vector2i;
    typedef Vector2<double> Vector2d;
    typedef Vector2<float> Vector2f;
    }
  }
}
#endif