File Vector3D.h

File List > include > mesh > Vector3D.h

Go to the documentation of this file

#pragma once

#include <cmath>
#include <limits>
#include <stdexcept>

namespace Argos {
template <typename T> class Vector3D {
public:
  T x;
  T y;
  T z;

  Vector3D();

  Vector3D(T x, T y, T z);

  Vector3D operator+(const Vector3D &vector) const;

  Vector3D &operator+=(const Vector3D &vector);

  Vector3D operator-(const Vector3D &vector) const;

  Vector3D &operator-=(const Vector3D &vector);

  Vector3D operator*(const T &scal) const;

  Vector3D &operator*=(const T &scal);

  Vector3D operator/(const T &scal) const;

  Vector3D &operator/=(const T &scal);

  T operator*(const Vector3D &vector) const;

  Vector3D operator^(const Vector3D &vector) const;

  Vector3D &operator^=(const Vector3D &vector);

  bool operator==(const Vector3D &vector) const;

  bool operator!=(const Vector3D &vector) const;

  T norm() const;

  T normSquared() const;

  Vector3D normalized() const;
};

template <typename T>
std::ostream &operator<<(std::ostream &os, const Vector3D<T> &v) {
  os << v.x << " " << v.y << " " << v.z;
  return os;
}
} // namespace Argos

template <typename T> Argos::Vector3D<T>::Vector3D() {
  x = 0;
  y = 0;
  z = 0;
}

template <typename T> Argos::Vector3D<T>::Vector3D(T x, T y, T z) {
  this->x = x;
  this->y = y;
  this->z = z;
}

template <typename T>
Argos::Vector3D<T> Argos::Vector3D<T>::operator+(const Vector3D &vector) const {
  return Vector3D(this->x + vector.x, this->y + vector.y, this->z + vector.z);
}

template <typename T>
Argos::Vector3D<T> &Argos::Vector3D<T>::operator+=(const Vector3D &vector) {
  this->x += vector.x;
  this->y += vector.y;
  this->z += vector.z;

  return *this;
}

template <typename T>
Argos::Vector3D<T> Argos::Vector3D<T>::operator-(const Vector3D &vector) const {
  return Vector3D(this->x - vector.x, this->y - vector.y, this->z - vector.z);
}

template <typename T>
Argos::Vector3D<T> &Argos::Vector3D<T>::operator-=(const Vector3D &vector) {
  this->x -= vector.x;
  this->y -= vector.y;
  this->z -= vector.z;

  return *this;
}

template <typename T>
Argos::Vector3D<T> Argos::Vector3D<T>::operator*(const T &scal) const {
  return Vector3D(this->x * scal, this->y * scal, this->z * scal);
}

template <typename T>
Argos::Vector3D<T> &Argos::Vector3D<T>::operator*=(const T &scal) {
  this->x *= scal;
  this->y *= scal;
  this->z *= scal;

  return *this;
}

template <typename T>
Argos::Vector3D<T> Argos::Vector3D<T>::operator/(const T &scal) const {
  if (scal == 0) {
    throw std::overflow_error("Division by zero !");
  }

  return Vector3D(this->x / scal, this->y / scal, this->z / scal);
}

template <typename T>
Argos::Vector3D<T> Argos::Vector3D<T>::operator^(const Vector3D &vector) const {
  return Vector3D(this->y * vector.z - this->z * vector.y,
                  this->z * vector.x - this->x * vector.z,
                  this->x * vector.y - this->y * vector.x);
}

template <typename T>
Argos::Vector3D<T> &Argos::Vector3D<T>::operator/=(const T &scal) {
  if (scal == 0) {
    throw std::overflow_error("Division by zero !");
  }

  this->x /= scal;
  this->y /= scal;
  this->z /= scal;

  return *this;
}

template <typename T>
T Argos::Vector3D<T>::operator*(const Vector3D &vector) const {
  return this->x * vector.x + this->y * vector.y + this->z * vector.z;
}

template <typename T>
Argos::Vector3D<T> &Argos::Vector3D<T>::operator^=(const Vector3D &vector) {
  T x = this->y * vector.z - this->z * vector.y;
  T y = this->x * vector.z - this->z * vector.x;
  T z = this->x * vector.y - this->y * vector.x;

  this->x = x;
  this->y = y;
  this->z = z;

  return *this;
}

template <typename T>
bool Argos::Vector3D<T>::operator==(const Vector3D &vector) const {
  float epsilon = std::numeric_limits<T>::epsilon();
  return std::abs(this->x - vector.x) < epsilon &&
         std::abs(this->y - vector.y) < epsilon &&
         std::abs(this->z - vector.z) < epsilon;
}

template <typename T>
bool Argos::Vector3D<T>::operator!=(const Vector3D &vector) const {
  float epsilon = std::numeric_limits<T>::epsilon();
  return std::abs(this->x - vector.x) >= epsilon &&
         std::abs(this->y - vector.y) >= epsilon &&
         std::abs(this->z - vector.z) >= epsilon;
}

template <typename T> T Argos::Vector3D<T>::norm() const {
  return std::sqrt(x * x + y * y + z * z);
}

template <typename T> T Argos::Vector3D<T>::normSquared() const {
  return x * x + y * y + z * z;
}

template <typename T>
Argos::Vector3D<T> Argos::Vector3D<T>::normalized() const {
  return *this / this->norm();
}

namespace std {
template <> struct hash<Argos::Vector3D<double>> {
  std::size_t operator()(const Argos::Vector3D<double> &v) const {
    std::size_t h1 = std::hash<double>{}(v.x);
    std::size_t h2 = std::hash<double>{}(v.y);
    std::size_t h3 = std::hash<double>{}(v.z);

    h1 ^= h2 + 0x9e3779b9 + (h1 << 6) + (h1 >> 2);
    h1 ^= h3 + 0x9e3779b9 + (h1 << 6) + (h1 >> 2);

    return h1;
  }
};
} // namespace std