dolphin/Source/Core/Common/Matrix.h

// Copyright 2019 Dolphin Emulator Project
// Licensed under GPLv2+
// Refer to the license.txt file included.

#pragma once

#include <array>
#include <cmath>
#include <functional>
#include <type_traits>

// Tiny matrix/vector library.
// Used for things like Free-Look in the gfx backend.

namespace Common
{
template <typename T>
union TVec3
{
  constexpr TVec3() = default;
  constexpr TVec3(T _x, T _y, T _z) : data{_x, _y, _z} {}

  template <typename OtherT>
  constexpr explicit TVec3(const TVec3<OtherT>& other) : TVec3(other.x, other.y, other.z)
  {
  }

  constexpr TVec3 Cross(const TVec3& rhs) const
  {
    return {(y * rhs.z) - (rhs.y * z), (z * rhs.x) - (rhs.z * x), (x * rhs.y) - (rhs.x * y)};
  }
  constexpr T Dot(const TVec3& other) const { return x * other.x + y * other.y + z * other.z; }
  constexpr T LengthSquared() const { return Dot(*this); }
  T Length() const { return std::sqrt(LengthSquared()); }
  TVec3 Normalized() const { return *this / Length(); }

  constexpr TVec3& operator+=(const TVec3& rhs)
  {
    x += rhs.x;
    y += rhs.y;
    z += rhs.z;
    return *this;
  }

  constexpr TVec3& operator-=(const TVec3& rhs)
  {
    x -= rhs.x;
    y -= rhs.y;
    z -= rhs.z;
    return *this;
  }

  constexpr TVec3& operator*=(const TVec3& rhs)
  {
    x *= rhs.x;
    y *= rhs.y;
    z *= rhs.z;
    return *this;
  }

  constexpr TVec3& operator/=(const TVec3& rhs)
  {
    x /= rhs.x;
    y /= rhs.y;
    z /= rhs.z;
    return *this;
  }

  constexpr TVec3 operator-() const { return {-x, -y, -z}; }

  // Apply function to each element and return the result.
  template <typename F>
  auto Map(F&& f) const -> TVec3<decltype(f(T{}))>
  {
    return {f(x), f(y), f(z)};
  }

  template <typename F, typename T2>
  auto Map(F&& f, const TVec3<T2>& t) const -> TVec3<decltype(f(T{}, t.x))>
  {
    return {f(x, t.x), f(y, t.y), f(z, t.z)};
  }

  template <typename F, typename T2>
  auto Map(F&& f, T2 scalar) const -> TVec3<decltype(f(T{}, scalar))>
  {
    return {f(x, scalar), f(y, scalar), f(z, scalar)};
  }

  std::array<T, 3> data = {};

  struct
  {
    T x;
    T y;
    T z;
  };
};

template <typename T>
TVec3<bool> operator<(const TVec3<T>& lhs, const TVec3<T>& rhs)
{
  return lhs.Map(std::less<T>{}, rhs);
}

constexpr TVec3<bool> operator!(const TVec3<bool>& vec)
{
  return {!vec.x, !vec.y, !vec.z};
}

template <typename T>
auto operator+(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x + rhs.x)>
{
  return lhs.Map(std::plus<decltype(lhs.x + rhs.x)>{}, rhs);
}

template <typename T>
auto operator-(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x - rhs.x)>
{
  return lhs.Map(std::minus<decltype(lhs.x - rhs.x)>{}, rhs);
}

template <typename T1, typename T2>
auto operator*(const TVec3<T1>& lhs, const TVec3<T2>& rhs) -> TVec3<decltype(lhs.x * rhs.x)>
{
  return lhs.Map(std::multiplies<decltype(lhs.x * rhs.x)>{}, rhs);
}

template <typename T>
auto operator/(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x / rhs.x)>
{
  return lhs.Map(std::divides<decltype(lhs.x / rhs.x)>{}, rhs);
}

template <typename T1, typename T2>
auto operator*(const TVec3<T1>& lhs, T2 scalar) -> TVec3<decltype(lhs.x * scalar)>
{
  return lhs.Map(std::multiplies<decltype(lhs.x * scalar)>{}, scalar);
}

template <typename T1, typename T2>
auto operator/(const TVec3<T1>& lhs, T2 scalar) -> TVec3<decltype(lhs.x / scalar)>
{
  return lhs.Map(std::divides<decltype(lhs.x / scalar)>{}, scalar);
}

using Vec3 = TVec3<float>;
using DVec3 = TVec3<double>;

template <typename T>
union TVec4
{
  constexpr TVec4() = default;
  constexpr TVec4(TVec3<T> _vec, T _w) : TVec4{_vec.x, _vec.y, _vec.z, _w} {}
  constexpr TVec4(T _x, T _y, T _z, T _w) : data{_x, _y, _z, _w} {}

  constexpr T Dot(const TVec4& other) const
  {
    return x * other.x + y * other.y + z * other.z + w * other.w;
  }

  constexpr TVec4& operator*=(const TVec4& rhs)
  {
    x *= rhs.x;
    y *= rhs.y;
    z *= rhs.z;
    w *= rhs.w;
    return *this;
  }

  constexpr TVec4& operator/=(const TVec4& rhs)
  {
    x /= rhs.x;
    y /= rhs.y;
    z /= rhs.z;
    w /= rhs.w;
    return *this;
  }

  constexpr TVec4& operator*=(T scalar) { return *this *= TVec4{scalar, scalar, scalar, scalar}; }
  constexpr TVec4& operator/=(T scalar) { return *this /= TVec4{scalar, scalar, scalar, scalar}; }

  std::array<T, 4> data = {};

  struct
  {
    T x;
    T y;
    T z;
    T w;
  };
};

template <typename T>
constexpr TVec4<T> operator*(TVec4<T> lhs, std::common_type_t<T> scalar)
{
  return lhs *= scalar;
}

template <typename T>
constexpr TVec4<T> operator/(TVec4<T> lhs, std::common_type_t<T> scalar)
{
  return lhs /= scalar;
}

using Vec4 = TVec4<float>;
using DVec4 = TVec4<double>;

template <typename T>
union TVec2
{
  constexpr TVec2() = default;
  constexpr TVec2(T _x, T _y) : data{_x, _y} {}

  template <typename OtherT>
  constexpr explicit TVec2(const TVec2<OtherT>& other) : TVec2(other.x, other.y)
  {
  }

  constexpr T Cross(const TVec2& rhs) const { return (x * rhs.y) - (y * rhs.x); }
  constexpr T Dot(const TVec2& rhs) const { return (x * rhs.x) + (y * rhs.y); }
  constexpr T LengthSquared() const { return Dot(*this); }
  T Length() const { return std::sqrt(LengthSquared()); }
  TVec2 Normalized() const { return *this / Length(); }

  constexpr TVec2& operator+=(const TVec2& rhs)
  {
    x += rhs.x;
    y += rhs.y;
    return *this;
  }

  constexpr TVec2& operator-=(const TVec2& rhs)
  {
    x -= rhs.x;
    y -= rhs.y;
    return *this;
  }

  constexpr TVec2& operator*=(const TVec2& rhs)
  {
    x *= rhs.x;
    y *= rhs.y;
    return *this;
  }

  constexpr TVec2& operator/=(const TVec2& rhs)
  {
    x /= rhs.x;
    y /= rhs.y;
    return *this;
  }

  constexpr TVec2& operator*=(T scalar)
  {
    x *= scalar;
    y *= scalar;
    return *this;
  }

  constexpr TVec2& operator/=(T scalar)
  {
    x /= scalar;
    y /= scalar;
    return *this;
  }

  constexpr TVec2 operator-() const { return {-x, -y}; }

  std::array<T, 2> data = {};

  struct
  {
    T x;
    T y;
  };
};

template <typename T>
constexpr TVec2<bool> operator<(const TVec2<T>& lhs, const TVec2<T>& rhs)
{
  return {lhs.x < rhs.x, lhs.y < rhs.y};
}

constexpr TVec2<bool> operator!(const TVec2<bool>& vec)
{
  return {!vec.x, !vec.y};
}

template <typename T>
constexpr TVec2<T> operator+(TVec2<T> lhs, const TVec2<T>& rhs)
{
  return lhs += rhs;
}

template <typename T>
constexpr TVec2<T> operator-(TVec2<T> lhs, const TVec2<T>& rhs)
{
  return lhs -= rhs;
}

template <typename T>
constexpr TVec2<T> operator*(TVec2<T> lhs, const TVec2<T>& rhs)
{
  return lhs *= rhs;
}

template <typename T>
constexpr TVec2<T> operator/(TVec2<T> lhs, const TVec2<T>& rhs)
{
  return lhs /= rhs;
}

template <typename T, typename T2>
constexpr auto operator*(TVec2<T> lhs, T2 scalar)
{
  return TVec2<decltype(lhs.x * scalar)>(lhs) *= scalar;
}

template <typename T, typename T2>
constexpr auto operator/(TVec2<T> lhs, T2 scalar)
{
  return TVec2<decltype(lhs.x / scalar)>(lhs) /= scalar;
}

using Vec2 = TVec2<float>;
using DVec2 = TVec2<double>;

class Matrix33;

class Quaternion
{
public:
  static Quaternion Identity();

  static Quaternion RotateX(float rad);
  static Quaternion RotateY(float rad);
  static Quaternion RotateZ(float rad);

  // Returns a quaternion with rotations about each axis simulatenously (e.g processing gyroscope
  // input)
  static Quaternion RotateXYZ(const Vec3& rads);

  static Quaternion Rotate(float rad, const Vec3& axis);

  Quaternion() = default;
  Quaternion(float w, float x, float y, float z);

  float Norm() const;
  Quaternion Normalized() const;
  Quaternion Conjugate() const;
  Quaternion Inverted() const;

  Quaternion& operator*=(const Quaternion& rhs);

  Vec4 data;
};

Quaternion operator*(Quaternion lhs, const Quaternion& rhs);
Vec3 operator*(const Quaternion& lhs, const Vec3& rhs);

class Matrix33
{
public:
  static Matrix33 Identity();
  static Matrix33 FromQuaternion(const Quaternion&);

  // Return a rotation matrix around the x,y,z axis
  static Matrix33 RotateX(float rad);
  static Matrix33 RotateY(float rad);
  static Matrix33 RotateZ(float rad);

  static Matrix33 Rotate(float rad, const Vec3& axis);

  static Matrix33 Scale(const Vec3& vec);

  // set result = a x b
  static void Multiply(const Matrix33& a, const Matrix33& b, Matrix33* result);
  static void Multiply(const Matrix33& a, const Vec3& vec, Vec3* result);

  Matrix33 Inverted() const;

  Matrix33& operator*=(const Matrix33& rhs)
  {
    Multiply(*this, rhs, this);
    return *this;
  }

  // Note: Row-major storage order.
  std::array<float, 9> data;
};

inline Matrix33 operator*(Matrix33 lhs, const Matrix33& rhs)
{
  return lhs *= rhs;
}

inline Vec3 operator*(const Matrix33& lhs, Vec3 rhs)
{
  Matrix33::Multiply(lhs, rhs, &rhs);
  return rhs;
}

class Matrix44
{
public:
  static Matrix44 Identity();
  static Matrix44 FromMatrix33(const Matrix33& m33);
  static Matrix44 FromQuaternion(const Quaternion& q);
  static Matrix44 FromArray(const std::array<float, 16>& arr);

  static Matrix44 Translate(const Vec3& vec);
  static Matrix44 Shear(const float a, const float b = 0);
  static Matrix44 Perspective(float fov_y, float aspect_ratio, float z_near, float z_far);

  static void Multiply(const Matrix44& a, const Matrix44& b, Matrix44* result);
  static void Multiply(const Matrix44& a, const Vec4& vec, Vec4* result);

  // For when a vec4 isn't needed a multiplication function that takes a Vec3 and w:
  Vec3 Transform(const Vec3& point, float w) const;

  Matrix44& operator*=(const Matrix44& rhs)
  {
    Multiply(*this, rhs, this);
    return *this;
  }

  // Note: Row-major storage order.
  std::array<float, 16> data;
};

inline Matrix44 operator*(Matrix44 lhs, const Matrix44& rhs)
{
  return lhs *= rhs;
}

inline Vec4 operator*(const Matrix44& lhs, Vec4 rhs)
{
  Matrix44::Multiply(lhs, rhs, &rhs);
  return rhs;
}

}  // namespace Common