Skip to content
Permalink
Browse files

WiimoteEmu: Reimplement tilt/swing/camera/orientation data using matr…

…ix math.
  • Loading branch information...
jordan-woyak committed Jan 29, 2019
1 parent 902e407 commit 4db4840d7c6ca42dd33c4bfc1e46568622e3a96b
@@ -9,21 +9,29 @@

namespace
{
void MatrixMul(int n, const float* a, const float* b, float* result)
// Multiply a NxM matrix by a NxP matrix.
template <int N, int M, int P, typename T>
auto MatrixMultiply(const std::array<T, N * M>& a, const std::array<T, M * P>& b)
-> std::array<T, N * P>
{
for (int i = 0; i < n; ++i)
std::array<T, N * P> result;

for (int n = 0; n != N; ++n)
{
for (int j = 0; j < n; ++j)
for (int p = 0; p != P; ++p)
{
float temp = 0;
for (int k = 0; k < n; ++k)
T temp = {};
for (int m = 0; m != M; ++m)
{
temp += a[i * n + k] * b[k * n + j];
temp += a[n * M + m] * b[m * P + p];
}
result[i * n + j] = temp;
result[n * P + p] = temp;
}
}

return result;
}

} // namespace

namespace Common
@@ -39,8 +47,8 @@ Matrix33 Matrix33::Identity()

Matrix33 Matrix33::RotateX(float rad)
{
const float s = sin(rad);
const float c = cos(rad);
const float s = std::sin(rad);
const float c = std::cos(rad);
Matrix33 mtx = {};
mtx.data[0] = 1;
mtx.data[4] = c;
@@ -52,8 +60,8 @@ Matrix33 Matrix33::RotateX(float rad)

Matrix33 Matrix33::RotateY(float rad)
{
const float s = sin(rad);
const float c = cos(rad);
const float s = std::sin(rad);
const float c = std::cos(rad);
Matrix33 mtx = {};
mtx.data[0] = c;
mtx.data[2] = s;
@@ -65,8 +73,8 @@ Matrix33 Matrix33::RotateY(float rad)

Matrix33 Matrix33::RotateZ(float rad)
{
const float s = sin(rad);
const float c = cos(rad);
const float s = std::sin(rad);
const float c = std::cos(rad);
Matrix33 mtx = {};
mtx.data[0] = c;
mtx.data[1] = -s;
@@ -87,20 +95,12 @@ Matrix33 Matrix33::Scale(const Vec3& vec)

void Matrix33::Multiply(const Matrix33& a, const Matrix33& b, Matrix33* result)
{
MatrixMul(3, a.data.data(), b.data.data(), result->data.data());
result->data = MatrixMultiply<3, 3, 3>(a.data, b.data);
}

void Matrix33::Multiply(const Matrix33& a, const Vec3& vec, Vec3* result)
{
for (int i = 0; i < 3; ++i)
{
result->data[i] = 0;

for (int k = 0; k < 3; ++k)
{
result->data[i] += a.data[i * 3 + k] * vec.data[k];
}
}
result->data = MatrixMultiply<3, 3, 1>(a.data, vec.data);
}

Matrix44 Matrix44::Identity()
@@ -157,8 +157,32 @@ Matrix44 Matrix44::Shear(const float a, const float b)
return mtx;
}

Matrix44 Matrix44::Perspective(float fov_y, float aspect_ratio, float z_near, float z_far)
{
Matrix44 mtx{};
const float tan_half_fov_y = std::tan(fov_y / 2);
mtx.data[0] = 1 / (aspect_ratio * tan_half_fov_y);
mtx.data[5] = 1 / tan_half_fov_y;
mtx.data[10] = -(z_far + z_near) / (z_far - z_near);
mtx.data[11] = -(2 * z_far * z_near) / (z_far - z_near);
mtx.data[14] = -1;
return mtx;
}

void Matrix44::Multiply(const Matrix44& a, const Matrix44& b, Matrix44* result)
{
MatrixMul(4, a.data.data(), b.data.data(), result->data.data());
result->data = MatrixMultiply<4, 4, 4>(a.data, b.data);
}

Vec3 Matrix44::Transform(const Vec3& v, float w) const
{
const auto result = MatrixMultiply<4, 4, 1>(data, {v.x, v.y, v.z, w});
return Vec3{result[0], result[1], result[2]};
}

void Matrix44::Multiply(const Matrix44& a, const Vec4& vec, Vec4* result)
{
result->data = MatrixMultiply<4, 4, 1>(a.data, vec.data);
}

} // namespace Common
@@ -6,40 +6,161 @@

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

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

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

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

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

std::array<float, 3> data = {};
TVec3& operator-=(const TVec3& rhs)
{
x -= rhs.x;
y -= rhs.y;
z -= rhs.z;
return *this;
}

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

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

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

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

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

inline Vec3 operator+(Vec3 lhs, const Vec3& rhs)
template <typename T>
TVec3<T> operator+(TVec3<T> lhs, const TVec3<T>& rhs)
{
return lhs += rhs;
}

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

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

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

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

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

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

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

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

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

TVec4& operator*=(T scalar) { return *this *= TVec4{scalar, scalar, scalar, scalar}; }
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>
TVec4<T> operator*(TVec4<T> lhs, std::common_type_t<T> scalar)
{
return lhs *= scalar;
}

template <typename T>
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
{
@@ -73,6 +194,13 @@ union TVec2
return *this;
}

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

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

std::array<T, 2> data = {};
@@ -102,6 +230,12 @@ TVec2<T> operator*(TVec2<T> lhs, T scalar)
return lhs *= scalar;
}

template <typename T>
TVec2<T> operator/(TVec2<T> lhs, T scalar)
{
return lhs /= scalar;
}

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

@@ -123,10 +257,11 @@ class Matrix33

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

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

@@ -135,11 +270,10 @@ inline Matrix33 operator*(Matrix33 lhs, const Matrix33& rhs)
return lhs *= rhs;
}

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

class Matrix44
@@ -151,20 +285,33 @@ class Matrix44

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(Matrix44(*this), rhs, this);
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

0 comments on commit 4db4840

Please sign in to comment.
You can’t perform that action at this time.