gz/math/Vector3.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2014 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 GZ_MATH_VECTOR3_HH_
#define GZ_MATH_VECTOR3_HH_
 
#include <algorithm>
#include <cmath>
#include <istream>
#include <limits>
#include <ostream>
 
#include <gz/math/Helpers.hh>
#include <gz/math/config.hh>
 
namespace gz::math
{
  // Inline bracket to help doxygen filtering.
  inline namespace GZ_MATH_VERSION_NAMESPACE {
  //
  template<typename T>
  class Vector3
  {
    public: static const Vector3 &Zero;
 
    public: static const Vector3 &One;
 
    public: static const Vector3 &UnitX;
 
    public: static const Vector3 &UnitY;
 
    public: static const Vector3 &UnitZ;
 
    public: static const Vector3 &NaN;
 
    public: constexpr Vector3()
    : data{0, 0, 0}
    {
    }
 
    public: constexpr Vector3(const T &_x, const T &_y, const T &_z)
    : data{_x, _y, _z}
    {
    }
 
    public: Vector3(const Vector3<T> &_v) = default;
 
    public: ~Vector3() = default;
 
    public: T Sum() const
    {
      return this->data[0] + this->data[1] + this->data[2];
    }
 
    public: T Distance(const Vector3<T> &_pt) const
    {
      return static_cast<T>(sqrt(
                  (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
                  (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
                  (this->data[2]-_pt[2])*(this->data[2]-_pt[2])));
    }
 
    public: T Distance(T _x, T _y, T _z) const
    {
      return this->Distance(Vector3(_x, _y, _z));
    }
 
    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] +
        this->data[2] * this->data[2];
    }
 
    public: Vector3 Normalize()
    {
      T d = this->Length();
 
      if (!equal<T>(d, static_cast<T>(0.0)))
      {
        this->data[0] /= d;
        this->data[1] /= d;
        this->data[2] /= d;
      }
 
      return *this;
    }
 
    public: Vector3 Normalized() const
    {
      Vector3<T> result = *this;
      result.Normalize();
      return result;
    }
 
    public: Vector3 Round()
    {
      this->data[0] = static_cast<T>(std::nearbyint(this->data[0]));
      this->data[1] = static_cast<T>(std::nearbyint(this->data[1]));
      this->data[2] = static_cast<T>(std::nearbyint(this->data[2]));
      return *this;
    }
 
    public: Vector3 Rounded() const
    {
      Vector3<T> result = *this;
      result.Round();
      return result;
    }
 
    public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
    {
      this->data[0] = _x;
      this->data[1] = _y;
      this->data[2] = _z;
    }
 
    public: Vector3 Cross(const Vector3<T> &_v) const
    {
      return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
                     this->data[2] * _v[0] - this->data[0] * _v[2],
                     this->data[0] * _v[1] - this->data[1] * _v[0]);
    }
 
    public: T Dot(const Vector3<T> &_v) const
    {
      return this->data[0] * _v[0] +
             this->data[1] * _v[1] +
             this->data[2] * _v[2];
    }
 
    public: T AbsDot(const Vector3<T> &_v) const
    {
      return std::abs(this->data[0] * _v[0]) +
             std::abs(this->data[1] * _v[1]) +
             std::abs(this->data[2] * _v[2]);
    }
 
    public: Vector3 Abs() const
    {
      return Vector3(std::abs(this->data[0]),
                     std::abs(this->data[1]),
                     std::abs(this->data[2]));
    }
 
    public: Vector3 Perpendicular() const
    {
      static const T sqrZero = static_cast<T>(1e-06 * 1e-06);
 
      Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
 
      // Check the length of the vector
      if (perp.SquaredLength() < sqrZero)
      {
        perp = this->Cross(Vector3(0, 1, 0));
      }
 
      return perp;
    }
 
    public: static Vector3 Normal(const Vector3<T> &_v1,
                const Vector3<T> &_v2, const Vector3<T> &_v3)
    {
      Vector3<T> a = _v2 - _v1;
      Vector3<T> b = _v3 - _v1;
      Vector3<T> n = a.Cross(b);
      return n.Normalize();
    }
 
    public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
    {
      T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
      d = d / (_pt2 - _pt1).Length();
      return d;
    }
 
    public: void Max(const Vector3<T> &_v)
    {
      if (_v[0] > this->data[0])
        this->data[0] = _v[0];
      if (_v[1] > this->data[1])
        this->data[1] = _v[1];
      if (_v[2] > this->data[2])
        this->data[2] = _v[2];
    }
 
    public: void Min(const Vector3<T> &_v)
    {
      if (_v[0] < this->data[0])
        this->data[0] = _v[0];
      if (_v[1] < this->data[1])
        this->data[1] = _v[1];
      if (_v[2] < this->data[2])
        this->data[2] = _v[2];
    }
 
    public: T Max() const
    {
      return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
    }
 
    public: T Min() const
    {
      return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
    }
 
    public: T MaxAbs() const
    {
      T max = std::max(std::abs(this->data[0]), std::abs(this->data[1]));
      max = std::max(max, std::abs(this->data[2]));
      return max;
    }
 
    public: T MinAbs() const
    {
      T min = std::min(std::abs(this->data[0]), std::abs(this->data[1]));
      min = std::min(min, std::abs(this->data[2]));
      return min;
    }
 
    public: Vector3 &operator=(const Vector3<T> &_v) = default;
 
    public: Vector3 &operator=(T _v)
    {
      this->data[0] = _v;
      this->data[1] = _v;
      this->data[2] = _v;
 
      return *this;
    }
 
    public: Vector3 operator+(const Vector3<T> &_v) const
    {
      return Vector3(this->data[0] + _v[0],
                     this->data[1] + _v[1],
                     this->data[2] + _v[2]);
    }
 
    public: const Vector3 &operator+=(const Vector3<T> &_v)
    {
      this->data[0] += _v[0];
      this->data[1] += _v[1];
      this->data[2] += _v[2];
 
      return *this;
    }
 
    public: inline Vector3<T> operator+(const T _s) const
    {
      return Vector3<T>(this->data[0] + _s,
                        this->data[1] + _s,
                        this->data[2] + _s);
    }
 
    public: friend inline Vector3<T> operator+(const T _s,
                                               const Vector3<T> &_v)
    {
      return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s};
    }
 
    public: const Vector3<T> &operator+=(const T _s)
    {
      this->data[0] += _s;
      this->data[1] += _s;
      this->data[2] += _s;
 
      return *this;
    }
 
    public: inline Vector3 operator-() const
    {
      return Vector3(-this->data[0], -this->data[1], -this->data[2]);
    }
 
    public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
    {
      return Vector3(this->data[0] - _pt[0],
                     this->data[1] - _pt[1],
                     this->data[2] - _pt[2]);
    }
 
    public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
    {
      this->data[0] -= _pt[0];
      this->data[1] -= _pt[1];
      this->data[2] -= _pt[2];
 
      return *this;
    }
 
    public: inline Vector3<T> operator-(const T _s) const
    {
      return Vector3<T>(this->data[0] - _s,
                        this->data[1] - _s,
                        this->data[2] - _s);
    }
 
    public: friend inline Vector3<T> operator-(const T _s,
                                               const Vector3<T> &_v)
    {
      return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()};
    }
 
    public: const Vector3<T> &operator-=(const T _s)
    {
      this->data[0] -= _s;
      this->data[1] -= _s;
      this->data[2] -= _s;
 
      return *this;
    }
 
    public: const Vector3<T> operator/(const Vector3<T> &_pt) const
    {
      return Vector3(this->data[0] / _pt[0],
                     this->data[1] / _pt[1],
                     this->data[2] / _pt[2]);
    }
 
    public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
    {
      this->data[0] /= _pt[0];
      this->data[1] /= _pt[1];
      this->data[2] /= _pt[2];
 
      return *this;
    }
 
    public: const Vector3<T> operator/(T _v) const
    {
      return Vector3(this->data[0] / _v,
                     this->data[1] / _v,
                     this->data[2] / _v);
    }
 
    public: const Vector3<T> &operator/=(T _v)
    {
      this->data[0] /= _v;
      this->data[1] /= _v;
      this->data[2] /= _v;
 
      return *this;
    }
 
    public: Vector3<T> operator*(const Vector3<T> &_p) const
    {
      return Vector3(this->data[0] * _p[0],
                     this->data[1] * _p[1],
                     this->data[2] * _p[2]);
    }
 
    public: const Vector3<T> &operator*=(const Vector3<T> &_v)
    {
      this->data[0] *= _v[0];
      this->data[1] *= _v[1];
      this->data[2] *= _v[2];
 
      return *this;
    }
 
    public: inline Vector3<T> operator*(T _s) const
    {
      return Vector3<T>(this->data[0] * _s,
                        this->data[1] * _s,
                        this->data[2] * _s);
    }
 
    public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
    {
      return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s};
    }
 
    public: const Vector3<T> &operator*=(T _v)
    {
      this->data[0] *= _v;
      this->data[1] *= _v;
      this->data[2] *= _v;
 
      return *this;
    }
 
    public: bool Equal(const Vector3 &_v, const T &_tol) const
    {
      return equal<T>(this->data[0], _v[0], _tol)
          && equal<T>(this->data[1], _v[1], _tol)
          && equal<T>(this->data[2], _v[2], _tol);
    }
 
    public: bool operator==(const Vector3<T> &_v) const
    {
      return this->Equal(_v, static_cast<T>(1e-3));
    }
 
    public: bool operator!=(const Vector3<T> &_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])) &&
             std::isfinite(static_cast<double>(this->data[2]));
    }
 
    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;
      if (!std::isfinite(static_cast<double>(this->data[2])))
        this->data[2] = 0;
    }
 
    public: T &operator[](const std::size_t _index)
    {
      return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
    }
 
    public: T operator[](const std::size_t _index) const
    {
      return this->data[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
    }
 
    public: void Round(int _precision)
    {
      this->data[0] = precision(this->data[0], _precision);
      this->data[1] = precision(this->data[1], _precision);
      this->data[2] = precision(this->data[2], _precision);
    }
 
    public: bool Equal(const Vector3<T> &_v) const
    {
      return equal<T>(this->data[0], _v[0]) &&
             equal<T>(this->data[1], _v[1]) &&
             equal<T>(this->data[2], _v[2]);
    }
 
    public: inline T X() const
    {
      return this->data[0];
    }
 
    public: inline T Y() const
    {
      return this->data[1];
    }
 
    public: inline T Z() const
    {
      return this->data[2];
    }
 
    public: inline T &X()
    {
      return this->data[0];
    }
 
    public: inline T &Y()
    {
      return this->data[1];
    }
 
    public: inline T &Z()
    {
      return this->data[2];
    }
 
    public: inline void X(const T &_v)
    {
      this->data[0] = _v;
    }
 
    public: inline void Y(const T &_v)
    {
      this->data[1] = _v;
    }
 
    public: inline void Z(const T &_v)
    {
      this->data[2] = _v;
    }
 
    public: bool operator<(const Vector3<T> &_pt) const
    {
      return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
             this->data[2] < _pt[2];
    }
 
    public: friend std::ostream &operator<<(
                std::ostream &_out, const gz::math::Vector3<T> &_pt)
    {
      for (auto i : {0, 1, 2})
      {
        if (i > 0)
          _out << " ";
 
        appendToStream(_out, _pt[i]);
      }
 
      return _out;
    }
 
    public: friend std::istream &operator>>(
                std::istream &_in, gz::math::Vector3<T> &_pt)
    {
      // Skip white spaces
      _in.setf(std::ios_base::skipws);
      T x, y, z;
      _in >> x >> y >> z;
      if (!_in.fail())
      {
        _pt.Set(x, y, z);
      }
      return _in;
    }
 
    private: T data[3];
  };
 
  namespace detail {
 
    template<typename T>
    constexpr Vector3<T> gVector3Zero(0, 0, 0);
    template<typename T>
    constexpr Vector3<T> gVector3One(1, 1, 1);
    template<typename T>
    constexpr Vector3<T> gVector3UnitX(1, 0, 0);
    template<typename T>
    constexpr Vector3<T> gVector3UnitY(0, 1, 0);
    template<typename T>
    constexpr Vector3<T> gVector3UnitZ(0, 0, 1);
    template<typename T>
    constexpr Vector3<T> gVector3NaN(
        std::numeric_limits<T>::quiet_NaN(),
        std::numeric_limits<T>::quiet_NaN(),
        std::numeric_limits<T>::quiet_NaN());
  }  // namespace detail
 
  template<typename T>
  const Vector3<T> &Vector3<T>::Zero = detail::gVector3Zero<T>;
  template<typename T>
  const Vector3<T> &Vector3<T>::One = detail::gVector3One<T>;
  template<typename T>
  const Vector3<T> &Vector3<T>::UnitX = detail::gVector3UnitX<T>;
  template<typename T>
  const Vector3<T> &Vector3<T>::UnitY = detail::gVector3UnitY<T>;
  template<typename T>
  const Vector3<T> &Vector3<T>::UnitZ = detail::gVector3UnitZ<T>;
  template<typename T>
  const Vector3<T> &Vector3<T>::NaN = detail::gVector3NaN<T>;
 
  typedef Vector3<int> Vector3i;
  typedef Vector3<double> Vector3d;
  typedef Vector3<float> Vector3f;
  }  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math
#endif  // GZ_MATH_VECTOR3_HH_