diff --git a/src/util/meson.build b/src/util/meson.build index e60818327..28f60f789 100644 --- a/src/util/meson.build +++ b/src/util/meson.build @@ -3,6 +3,7 @@ util_src = files([ 'util_string.cpp', 'util_gdi.cpp', 'util_luid.cpp', + 'util_matrix.cpp', 'com/com_guid.cpp', 'com/com_private_data.cpp', diff --git a/src/util/util_matrix.cpp b/src/util/util_matrix.cpp new file mode 100644 index 000000000..1de361c63 --- /dev/null +++ b/src/util/util_matrix.cpp @@ -0,0 +1,255 @@ +#include "util_matrix.h" + +namespace dxvk { + + // Identity + Matrix4::Matrix4() { + data[0] = { 1, 0, 0, 0 }; + data[1] = { 0, 1, 0, 0 }; + data[2] = { 0, 0, 1, 0 }; + data[3] = { 0, 0, 0, 1 }; + } + + // Produces a scalar matrix, x * Identity + Matrix4::Matrix4(float x) { + data[0] = { x, 0, 0, 0 }; + data[1] = { 0, x, 0, 0 }; + data[2] = { 0, 0, x, 0 }; + data[3] = { 0, 0, 0, x }; + } + + Matrix4::Matrix4( + const Vector4& v0, + const Vector4& v1, + const Vector4& v2, + const Vector4& v3) { + data[0] = v0; + data[1] = v1; + data[2] = v2; + data[3] = v3; + } + + Vector4& Matrix4::operator[](size_t index) { return data[index]; } + const Vector4& Matrix4::operator[](size_t index) const { return data[index]; } + + bool Matrix4::operator==(const Matrix4& m2) const { + const Matrix4& m1 = *this; + for (uint32_t i = 0; i < 4; i++) { + if (m1[i] != m2[i]) + return false; + } + return true; + } + + bool Matrix4::operator!=(const Matrix4& m2) const { return !operator==(m2); } + + Matrix4 Matrix4::operator+(const Matrix4& other) const { + Matrix4 mat; + for (uint32_t i = 0; i < 4; i++) + mat[i] = data[i] + other.data[i]; + return mat; + } + + Matrix4 Matrix4::operator-(const Matrix4& other) const { + Matrix4 mat; + for (uint32_t i = 0; i < 4; i++) + mat[i] = data[i] - other.data[i]; + return mat; + } + + Matrix4 Matrix4::operator*(const Matrix4& m2) const { + const Matrix4& m1 = *this; + + const Vector4 srcA0 = { m1[0] }; + const Vector4 srcA1 = { m1[1] }; + const Vector4 srcA2 = { m1[2] }; + const Vector4 srcA3 = { m1[3] }; + + const Vector4 srcB0 = { m2[0] }; + const Vector4 srcB1 = { m2[1] }; + const Vector4 srcB2 = { m2[2] }; + const Vector4 srcB3 = { m2[3] }; + + Matrix4 result; + result[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + result[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + result[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + result[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return result; + } + + Vector4 Matrix4::operator*(const Vector4& v) const { + const Matrix4& m = *this; + + const Vector4 mul0 = { m[0] * v[0] }; + const Vector4 mul1 = { m[1] * v[1] }; + const Vector4 mul2 = { m[2] * v[2] }; + const Vector4 mul3 = { m[3] * v[3] }; + + const Vector4 add0 = { mul0 + mul1 }; + const Vector4 add1 = { mul2 + mul3 }; + + return add0 + add1; + } + + Matrix4 Matrix4::operator*(float scalar) const { + Matrix4 mat; + for (uint32_t i = 0; i < 4; i++) + mat[i] = data[i] * scalar; + return mat; + } + + Matrix4 Matrix4::operator/(float scalar) const { + Matrix4 mat; + for (uint32_t i = 0; i < 4; i++) + mat[i] = data[i] / scalar; + return mat; + } + + Matrix4& Matrix4::operator+=(const Matrix4& other) { + return (*this = (*this) + other); + } + + Matrix4& Matrix4::operator-=(const Matrix4& other) { + return (*this = (*this) - other); + } + + Matrix4& Matrix4::operator*=(const Matrix4& other) { + return (*this = (*this) * other); + } + + Matrix4 transpose(const Matrix4& m) { + Matrix4 result; + + for (uint32_t i = 0; i < 4; i++) { + for (uint32_t j = 0; j < 4; j++) + result[i][j] = m.data[j][i]; + } + return result; + } + + float determinant(const Matrix4& m) { + float coef00 = m[2][2] * m[3][3] - m[3][2] * m[2][3]; + float coef02 = m[1][2] * m[3][3] - m[3][2] * m[1][3]; + float coef03 = m[1][2] * m[2][3] - m[2][2] * m[1][3]; + + float coef04 = m[2][1] * m[3][3] - m[3][1] * m[2][3]; + float coef06 = m[1][1] * m[3][3] - m[3][1] * m[1][3]; + float coef07 = m[1][1] * m[2][3] - m[2][1] * m[1][3]; + + float coef08 = m[2][1] * m[3][2] - m[3][1] * m[2][2]; + float coef10 = m[1][1] * m[3][2] - m[3][1] * m[1][2]; + float coef11 = m[1][1] * m[2][2] - m[2][1] * m[1][2]; + + float coef12 = m[2][0] * m[3][3] - m[3][0] * m[2][3]; + float coef14 = m[1][0] * m[3][3] - m[3][0] * m[1][3]; + float coef15 = m[1][0] * m[2][3] - m[2][0] * m[1][3]; + + float coef16 = m[2][0] * m[3][2] - m[3][0] * m[2][2]; + float coef18 = m[1][0] * m[3][2] - m[3][0] * m[1][2]; + float coef19 = m[1][0] * m[2][2] - m[2][0] * m[1][2]; + + float coef20 = m[2][0] * m[3][1] - m[3][0] * m[2][1]; + float coef22 = m[1][0] * m[3][1] - m[3][0] * m[1][1]; + float coef23 = m[1][0] * m[2][1] - m[2][0] * m[1][1]; + + Vector4 fac0 = { coef00, coef00, coef02, coef03 }; + Vector4 fac1 = { coef04, coef04, coef06, coef07 }; + Vector4 fac2 = { coef08, coef08, coef10, coef11 }; + Vector4 fac3 = { coef12, coef12, coef14, coef15 }; + Vector4 fac4 = { coef16, coef16, coef18, coef19 }; + Vector4 fac5 = { coef20, coef20, coef22, coef23 }; + + Vector4 vec0 = { m[1][0], m[0][0], m[0][0], m[0][0] }; + Vector4 vec1 = { m[1][1], m[0][1], m[0][1], m[0][1] }; + Vector4 vec2 = { m[1][2], m[0][2], m[0][2], m[0][2] }; + Vector4 vec3 = { m[1][3], m[0][3], m[0][3], m[0][3] }; + + Vector4 inv0 = { vec1 * fac0 - vec2 * fac1 + vec3 * fac2 }; + Vector4 inv1 = { vec0 * fac0 - vec2 * fac3 + vec3 * fac4 }; + Vector4 inv2 = { vec0 * fac1 - vec1 * fac3 + vec3 * fac5 }; + Vector4 inv3 = { vec0 * fac2 - vec1 * fac4 + vec2 * fac5 }; + + Vector4 signA = { +1, -1, +1, -1 }; + Vector4 signB = { -1, +1, -1, +1 }; + Matrix4 inverse = { inv0 * signA, inv1 * signB, inv2 * signA, inv3 * signB }; + + Vector4 row0 = { inverse[0][0], inverse[1][0], inverse[2][0], inverse[3][0] }; + + Vector4 dot0 = { m[0] * row0 }; + + return (dot0.x + dot0.y) + (dot0.z + dot0.w); + } + + Matrix4 inverse(const Matrix4& m) + { + float coef00 = m[2][2] * m[3][3] - m[3][2] * m[2][3]; + float coef02 = m[1][2] * m[3][3] - m[3][2] * m[1][3]; + float coef03 = m[1][2] * m[2][3] - m[2][2] * m[1][3]; + float coef04 = m[2][1] * m[3][3] - m[3][1] * m[2][3]; + float coef06 = m[1][1] * m[3][3] - m[3][1] * m[1][3]; + float coef07 = m[1][1] * m[2][3] - m[2][1] * m[1][3]; + float coef08 = m[2][1] * m[3][2] - m[3][1] * m[2][2]; + float coef10 = m[1][1] * m[3][2] - m[3][1] * m[1][2]; + float coef11 = m[1][1] * m[2][2] - m[2][1] * m[1][2]; + float coef12 = m[2][0] * m[3][3] - m[3][0] * m[2][3]; + float coef14 = m[1][0] * m[3][3] - m[3][0] * m[1][3]; + float coef15 = m[1][0] * m[2][3] - m[2][0] * m[1][3]; + float coef16 = m[2][0] * m[3][2] - m[3][0] * m[2][2]; + float coef18 = m[1][0] * m[3][2] - m[3][0] * m[1][2]; + float coef19 = m[1][0] * m[2][2] - m[2][0] * m[1][2]; + float coef20 = m[2][0] * m[3][1] - m[3][0] * m[2][1]; + float coef22 = m[1][0] * m[3][1] - m[3][0] * m[1][1]; + float coef23 = m[1][0] * m[2][1] - m[2][0] * m[1][1]; + + Vector4 fac0 = { coef00, coef00, coef02, coef03 }; + Vector4 fac1 = { coef04, coef04, coef06, coef07 }; + Vector4 fac2 = { coef08, coef08, coef10, coef11 }; + Vector4 fac3 = { coef12, coef12, coef14, coef15 }; + Vector4 fac4 = { coef16, coef16, coef18, coef19 }; + Vector4 fac5 = { coef20, coef20, coef22, coef23 }; + + Vector4 vec0 = { m[1][0], m[0][0], m[0][0], m[0][0] }; + Vector4 vec1 = { m[1][1], m[0][1], m[0][1], m[0][1] }; + Vector4 vec2 = { m[1][2], m[0][2], m[0][2], m[0][2] }; + Vector4 vec3 = { m[1][3], m[0][3], m[0][3], m[0][3] }; + + Vector4 inv0 = { vec1 * fac0 - vec2 * fac1 + vec3 * fac2 }; + Vector4 inv1 = { vec0 * fac0 - vec2 * fac3 + vec3 * fac4 }; + Vector4 inv2 = { vec0 * fac1 - vec1 * fac3 + vec3 * fac5 }; + Vector4 inv3 = { vec0 * fac2 - vec1 * fac4 + vec2 * fac5 }; + + Vector4 signA = { +1, -1, +1, -1 }; + Vector4 signB = { -1, +1, -1, +1 }; + Matrix4 inverse = { inv0 * signA, inv1 * signB, inv2 * signA, inv3 * signB }; + + Vector4 row0 = { inverse[0][0], inverse[1][0], inverse[2][0], inverse[3][0] }; + + Vector4 dot0 = { m[0] * row0 }; + float dot1 = (dot0.x + dot0.y) + (dot0.z + dot0.w); + + return inverse * (1.0f / dot1); + } + + Matrix4 hadamardProduct(const Matrix4& a, const Matrix4& b) { + Matrix4 result; + + for (uint32_t i = 0; i < 4; i++) + result[i] = a[i] * b[i]; + + return result; + } + + std::ostream& operator<<(std::ostream& os, const Matrix4& m) { + os << "Matrix4("; + for (uint32_t i = 0; i < 4; i++) { + os << "\n\t" << m[i]; + if (i < 3) + os << ", "; + } + os << "\n)"; + + return os; + } + +} \ No newline at end of file diff --git a/src/util/util_matrix.h b/src/util/util_matrix.h new file mode 100644 index 000000000..bd623e615 --- /dev/null +++ b/src/util/util_matrix.h @@ -0,0 +1,59 @@ +#pragma once + +#include "util_vector.h" + +namespace dxvk { + + class Matrix4 { + + public: + + Matrix4(); // Identity + + explicit Matrix4(float x); // Produces a scalar matrix, x * Identity + + Matrix4( + const Vector4& v0, + const Vector4& v1, + const Vector4& v2, + const Vector4& v3); + + Matrix4(const Matrix4& other) = default; + + Vector4& operator[](size_t index); + const Vector4& operator[](size_t index) const; + + bool operator==(const Matrix4& m2) const; + bool operator!=(const Matrix4& m2) const; + + Matrix4 operator+(const Matrix4& other) const; + Matrix4 operator-(const Matrix4& other) const; + + Matrix4 operator*(const Matrix4& m2) const; + Vector4 operator*(const Vector4& v) const; + Matrix4 operator*(float scalar) const; + + Matrix4 operator/(float scalar) const; + + Matrix4& operator+=(const Matrix4& other); + Matrix4& operator-=(const Matrix4& other); + + Matrix4& operator*=(const Matrix4& other); + + Vector4 data[4]; + + }; + + inline Matrix4 operator*(float scalar, const Matrix4& m) { return m * scalar; } + + Matrix4 transpose(const Matrix4& m); + + float determinant(const Matrix4& m); + + Matrix4 inverse(const Matrix4& m); + + Matrix4 hadamardProduct(const Matrix4& a, const Matrix4& b); + + std::ostream& operator<<(std::ostream& os, const Matrix4& m); + +} \ No newline at end of file diff --git a/src/util/util_vector.h b/src/util/util_vector.h new file mode 100644 index 000000000..d25dd2f11 --- /dev/null +++ b/src/util/util_vector.h @@ -0,0 +1,158 @@ +#pragma once + +#include + +#include "util_bit.h" +#include "util_math.h" + +namespace dxvk { + + template + struct Vector4Base { + Vector4Base() + : x{ }, y{ }, z{ }, w{ } { } + + Vector4Base(T splat) + : x(splat), y(splat), z(splat), w(splat) { } + + Vector4Base(T x, T y, T z, T w) + : x(x), y(y), z(z), w(w) { } + + Vector4Base(T xyzw[4]) + : x(xyzw[0]), y(xyzw[1]), z(xyzw[2]), w(xyzw[3]) { } + + Vector4Base(const Vector4Base& other) = default; + + inline float& operator[](size_t index) { return data[index]; } + inline const float& operator[](size_t index) const { return data[index]; } + + bool operator==(const Vector4Base& other) const { + for (uint32_t i = 0; i < 4; i++) { + if (data[i] != other.data[i]) + return false; + } + + return true; + } + + bool operator!=(const Vector4Base& other) const { + return !operator==(other); + } + + Vector4Base operator-() const { return {-x, -y, -z, -w}; } + + Vector4Base operator+(const Vector4Base& other) const { + return {x + other.x, y + other.y, z + other.z, w + other.w}; + } + + Vector4Base operator-(const Vector4Base& other) const { + return {x - other.x, y - other.y, z - other.z, w - other.w}; + } + + Vector4Base operator*(T scalar) const { + return {scalar * x, scalar * y, scalar * z, scalar * w}; + } + + Vector4Base operator*(const Vector4Base& other) const { + Vector4Base result; + for (uint32_t i = 0; i < 4; i++) + result[i] = data[i] * other.data[i]; + return result; + } + + Vector4Base operator/(const Vector4Base& other) const { + Vector4Base result; + for (uint32_t i = 0; i < 4; i++) + result[i] = data[i] / other.data[i]; + return result; + } + + Vector4Base operator/(T scalar) const { + return {x / scalar, y / scalar, z / scalar, w / scalar}; + } + + Vector4Base& operator+=(const Vector4Base& other) { + x += other.x; + y += other.y; + z += other.z; + w += other.w; + + return *this; + } + + Vector4Base& operator-=(const Vector4Base& other) { + x -= other.x; + y -= other.y; + z -= other.z; + w -= other.w; + + return *this; + } + + Vector4Base& operator*=(T scalar) { + x *= scalar; + y *= scalar; + z *= scalar; + w *= scalar; + + return *this; + } + + Vector4Base& operator/=(T scalar) { + x /= scalar; + y /= scalar; + z /= scalar; + w /= scalar; + + return *this; + } + + union { + T data[4]; + struct { + T x, y, z, w; + }; + struct { + T r, g, b, a; + }; + }; + + }; + + template + inline Vector4Base operator*(T scalar, const Vector4Base& vector) { + return vector * scalar; + } + + template + float dot(const Vector4Base& a, const Vector4Base& b) { + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; + } + + template + T lengthSqr(const Vector4Base& a) { return dot(a, a); } + + template + float length(const Vector4Base& a) { return std::sqrt(float(lengthSqr(a))); } + + template + Vector4Base normalize(const Vector4Base& a) { return a * T(1.0f / length(a)); } + + template + std::ostream& operator<<(std::ostream& os, const Vector4Base& v) { + return os << "Vector4(" << v[0] << ", " << v[1] << ", " << v[2] << ", " << v[3] << ")"; + } + + using Vector4 = Vector4Base; + using Vector4i = Vector4Base; + + inline Vector4 replaceNaN(Vector4 a) { + Vector4 result; + __m128 value = _mm_load_ps(a.data); + __m128 mask = _mm_cmpeq_ps(value, value); + value = _mm_and_ps(value, mask); + _mm_store_ps(result.data, value); + return result; + } + +} \ No newline at end of file