VectorOps.h

/*
 * Gamedev Framework (gf)
 * Copyright (C) 2016-2019 Julien Bernard
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
#ifndef GF_VECTOR_OPS_H
#define GF_VECTOR_OPS_H

#include <cmath>
#include <cstdlib>
#include <algorithm>
#include <type_traits>

#include "Math.h"
#include "Vector.h"

namespace gf {
#ifndef DOXYGEN_SHOULD_SKIP_THIS
inline namespace v1 {
#endif

  template<typename T, std::size_t N>
  constexpr
  bool operator==(Vector<T, N> lhs, Vector<T, N> rhs) {
    for (std::size_t i = 0; i < N; ++i) {
      if (lhs[i] != rhs[i]) {
        return false;
      }
    }

    return true;
  }

  template<typename T, std::size_t N>
  constexpr
  bool operator!=(Vector<T, N> lhs, Vector<T, N> rhs) {
    return !(lhs == rhs);
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<T, N> operator-(Vector<T, N> val) {
    Vector<T, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = - val[i];
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator+(Vector<T, N> lhs, Vector<U, N> rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] + rhs[i];
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<T, N>& operator+=(Vector<T, N>& lhs, Vector<U, N> rhs) {
    for (std::size_t i = 0; i < N; ++i) {
      lhs[i] += rhs[i];
    }

    return lhs;
  }

  template<typename T, typename U, std::size_t N, typename E = typename std::enable_if<std::is_arithmetic<U>::value, U>::type>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator+(Vector<T, N> lhs, U rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] + rhs;
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<T, N>& operator+=(Vector<T, N>& lhs, U rhs) {
    for (std::size_t i = 0; i < N; ++i) {
      lhs[i] += rhs;
    }

    return lhs;
  }

  template<typename T, typename U, std::size_t N, typename E = typename std::enable_if<std::is_arithmetic<T>::value, T>::type>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator+(T lhs, Vector<U, N> rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs + rhs[i];
    }

    return out;
  }


  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator-(Vector<T, N> lhs, Vector<U, N> rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] - rhs[i];
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<T, N>& operator-=(Vector<T, N>& lhs, Vector<U, N> rhs) {
    for (std::size_t i = 0; i < N; ++i) {
      lhs[i] -= rhs[i];
    }

    return lhs;
  }


  template<typename T, typename U, std::size_t N, typename E = typename std::enable_if<std::is_arithmetic<U>::value, U>::type>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator-(Vector<T, N> lhs, U rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] - rhs;
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<T, N>& operator-=(Vector<T, N>& lhs, U rhs) {
    for (std::size_t i = 0; i < N; ++i) {
      lhs[i] -= rhs;
    }

    return lhs;
  }

  template<typename T, typename U, std::size_t N, typename E = typename std::enable_if<std::is_arithmetic<T>::value, T>::type>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator-(T lhs, Vector<U, N> rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs - rhs[i];
    }

    return out;
  }


  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator*(Vector<T, N> lhs, Vector<U, N> rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] * rhs[i];
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<T, N>& operator*=(Vector<T, N>& lhs, Vector<U, N> rhs) {
    for (std::size_t i = 0; i < N; ++i) {
      lhs[i] *= rhs[i];
    }

    return lhs;
  }

  template<typename T, typename U, std::size_t N, typename E = typename std::enable_if<std::is_arithmetic<U>::value, U>::type>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator*(Vector<T, N> lhs, U rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] * rhs;
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<T, N>& operator*=(Vector<T, N>& lhs, U rhs) {
    for (std::size_t i = 0; i < N; ++i) {
      lhs[i] *= rhs;
    }

    return lhs;
  }

  template<typename T, typename U, std::size_t N, typename E = typename std::enable_if<std::is_arithmetic<T>::value, T>::type>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator*(T lhs, Vector<U, N> rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs * rhs[i];
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator/(Vector<T, N> lhs, Vector<U, N> rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] / rhs[i];
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<T, N>& operator/=(Vector<T, N>& lhs, Vector<U, N> rhs) {
    for (std::size_t i = 0; i < N; ++i) {
      lhs[i] /= rhs[i];
    }

    return lhs;
  }

  template<typename T, typename U, std::size_t N, typename E = typename std::enable_if<std::is_arithmetic<U>::value, U>::type>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator/(Vector<T, N> lhs, U rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] / rhs;
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<T, N>& operator/=(Vector<T, N>& lhs, U rhs) {
    for (std::size_t i = 0; i < N; ++i) {
      lhs[i] /= rhs;
    }

    return lhs;
  }

  template<typename T, typename U, std::size_t N, typename E = typename std::enable_if<std::is_arithmetic<T>::value, T>::type>
  constexpr
  Vector<std::common_type_t<T,U>, N> operator/(T lhs, Vector<U, N> rhs) {
    Vector<std::common_type_t<T,U>, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs / rhs[i];
    }

    return out;
  }

  template<std::size_t N>
  constexpr
  Vector<bool, N> operator||(Vector<bool, N> lhs, Vector<bool, N> rhs) {
    Vector<bool, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] || rhs[i];
    }

    return out;
  }

  template<std::size_t N>
  constexpr
  Vector<bool, N> operator&&(Vector<bool, N> lhs, Vector<bool, N> rhs) {
    Vector<bool, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lhs[i] && rhs[i];
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  T dot(Vector<T, N> lhs, Vector<T, N> rhs) {
    T out{0};

    for (std::size_t i = 0; i < N; ++i) {
      out += lhs[i] * rhs[i];
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<T, N> min(Vector<T, N> lhs, Vector<T, N> rhs) {
    Vector<T, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = std::min(lhs[i], rhs[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<T, N> max(Vector<T, N> lhs, Vector<T, N> rhs) {
    Vector<T, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = std::max(lhs[i], rhs[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  inline
  Vector<T, N> abs(Vector<T, N> val) {
    Vector<T, N> out;

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = std::abs(val[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<int, N> sign(Vector<T, N> val) {
    Vector<int, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = gf::sign(val[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<bool, N> equals(Vector<T, N> lhs, Vector<T, N> rhs) {
    Vector<bool, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = (lhs[i] == rhs[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<bool, N> lessThan(Vector<T, N> lhs, Vector<T, N> rhs) {
    Vector<bool, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = (lhs[i] < rhs[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<bool, N> greaterThan(Vector<T, N> lhs, Vector<T, N> rhs) {
    Vector<bool, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = (lhs[i] > rhs[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<T, N> select(Vector<bool, N> cond, Vector<T, N> lhs, Vector<T, N> rhs) {
    Vector<T, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = (cond[i] ? lhs[i] : rhs[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<T, N> clamp(Vector<T, N> val, Vector<T, N> lo, Vector<T, N> hi) {
    Vector<T, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = clamp(val[i], lo[i], hi[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  constexpr
  Vector<T, N> clamp(Vector<T, N> val, T lo, T hi) {
    Vector<T, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = clamp(val[i], lo, hi);
    }

    return out;
  }

  template<typename T, typename U, std::size_t N>
  constexpr
  Vector<T, N> lerp(Vector<T, N> lhs, Vector<T, N> rhs, U t) {
    Vector<T, N> out = { };

    for (std::size_t i = 0; i < N; ++i) {
      out[i] = lerp(lhs[i], rhs[i], t);
    }

    return out;
    // return (1 - t) * lhs + t * rhs;
  }

  template<typename T, std::size_t N>
  inline
  T manhattanLength(Vector<T, N> vec) {
    T out{0};

    for (std::size_t i = 0; i < N; ++i) {
      out += std::abs(vec[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  inline
  T squareLength(Vector<T, N> vec) {
    T out{0};

    for (std::size_t i = 0; i < N; ++i) {
      out += square(vec[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  inline
  T euclideanLength(Vector<T, N> vec) {
    return std::sqrt(squareLength(vec));
  }

#ifndef DOXYGEN_SHOULD_SKIP_THIS
  // specializations of euclideanLength for Vector2f and Vector2d using std::hypot

  template<>
  inline
  float euclideanLength<float, 2>(Vector<float, 2> vec) {
    return std::hypot(vec.x, vec.y);
  }

  template<>
  inline
  double euclideanLength<double, 2>(Vector<double, 2> vec) {
    return std::hypot(vec.x, vec.y);
  }
#endif

  template<typename T, std::size_t N>
  inline
  T chebyshevLength(Vector<T, N> vec) {
    T out = std::abs(vec[0]);

    for (std::size_t i = 1; i < N; ++i) {
      out = std::max(out, std::abs(vec[i]));
    }

    return out;
  }

  template<typename T, std::size_t N>
  inline
  T naturalLength(Vector<T, N> vec) {
    return manhattanLength(vec) + squareLength(vec);
  }

  template<typename T, std::size_t N>
  inline
  T manhattanDistance(Vector<T, N> lhs, Vector<T, N> rhs) {
    return manhattanLength(lhs - rhs);
  }

  template<typename T, std::size_t N>
  inline
  T squareDistance(Vector<T, N> lhs, Vector<T, N> rhs) {
    return squareLength(lhs - rhs);
  }

  template<typename T, std::size_t N>
  inline
  T euclideanDistance(Vector<T, N> lhs, Vector<T, N> rhs) {
    return euclideanLength(lhs - rhs);
  }

  template<typename T, std::size_t N>
  inline
  T chebyshevDistance(Vector<T, N> lhs, Vector<T, N> rhs) {
    return chebyshevLength(lhs - rhs);
  }

  template<typename T, std::size_t N>
  inline
  T naturalDistance(Vector<T, N> lhs, Vector<T, N> rhs) {
    return naturalLength(lhs - rhs);
  }

  template<typename T, std::size_t N>
  inline
  Vector<T, N> normalize(Vector<T, N> vec) {
    T length = euclideanLength(vec);
    return vec / length;
  }

  template<typename T>
  constexpr
  Vector<T, 2> projx(Vector<T, 2> vec) {
    return { vec.x, T(0) };
  }

  template<typename T>
  constexpr
  Vector<T, 2> projy(Vector<T, 2> vec) {
    return { T(0), vec.y };
  }

  template<typename T>
  constexpr
  Vector<T, 2> dirx(T length) {
    return { length, T(0) };
  }

  template<typename T>
  constexpr
  Vector<T, 2> diry(T length) {
    return { T(0), length };
  }

  template<typename T>
  inline
  Vector<T, 2> unit(T angle) {
    return { std::cos(angle), std::sin(angle) };
  }

  template<typename T>
  inline
  float angle(Vector<T, 2> vec) {
    return std::atan2(vec.y, vec.x);
  }

  template<typename T>
  constexpr
  Vector<T, 2> perp(Vector<T, 2> vec) {
    return { -vec.y, vec.x };
  }

  template<typename T>
  constexpr
  Vector<T, 2> vectorTripleProduct(Vector<T, 2> a, Vector<T, 2> b, Vector<T, 2> c) {
    return dot(a, c) * b - dot(a, b) * c;
  }

  template<typename T>
  constexpr
  Vector<T, 2> inverseVectorTripleProduct(Vector<T, 2> a, Vector<T, 2> b, Vector<T, 2> c) {
    return - dot(c, b) * a + dot(c, a) * b;
  }

  template<typename T>
  constexpr
  T cross(Vector<T, 2> lhs, Vector<T, 2> rhs) {
    return lhs.x * rhs.y - lhs.y * rhs.x;
  }


  template<typename T>
  constexpr
  Vector<T, 3> cross(Vector<T, 3> lhs, Vector<T, 3> rhs) {
    return {
      lhs.y * rhs.z - lhs.z * rhs.y,
      lhs.z * rhs.x - lhs.x * rhs.z,
      lhs.x * rhs.y - lhs.y * rhs.x
    };
  }


  template<typename Archive, typename T, std::size_t N>
  inline
  Archive& operator|(Archive& ar, Vector<T,N>& vec) {
    for (std::size_t i = 0; i < N; ++i) {
      ar | vec[i];
    }

    return ar;
  }

#ifndef DOXYGEN_SHOULD_SKIP_THIS
}
#endif
}

#endif // GF_VECTOR_OPS_H