#pragma once #include "StarVector.hpp" namespace Star { template <typename T> class Matrix3 { public: typedef Vector<T, 3> Vec3; typedef Vector<T, 2> Vec2; typedef Array<Vec3, 3> Rows; // Only enable pointer access if we know that our internal rows are not // padded template <typename RT = void> using EnableIfContiguousStorage = typename std::enable_if<sizeof(Vec3) == 3 * sizeof(T) && sizeof(Rows) == 3 * sizeof(Vec3), RT>::type; static Matrix3 identity(); // Construct an affine 2d transform static Matrix3 rotation(T angle, Vec2 const& point = Vec2()); static Matrix3 translation(Vec2 const& point); static Matrix3 scaling(T scale, Vec2 const& point = Vec2()); static Matrix3 scaling(Vec2 const& scale, Vec2 const& point = Vec2()); Matrix3(); Matrix3(T r1c1, T r1c2, T r1c3, T r2c1, T r2c2, T r2c3, T r3c1, T r3c2, T r3c3); Matrix3(Vec3 const& r1, Vec3 const& r2, Vec3 const& r3); Matrix3(T const* ptr); template <typename T2> Matrix3(Matrix3<T2> const& m); template <typename T2> Matrix3& operator=(Matrix3<T2> const& m); // Row-major indexing Vec3& operator[](size_t const i); Vec3 const& operator[](size_t const i) const; // Gives pointer to row major storage EnableIfContiguousStorage<T*> ptr(); EnableIfContiguousStorage<T const*> ptr() const; // Copy to an existing array void copy(T* loc) const; Vec3 row(size_t i) const; template <typename T2> void setRow(size_t i, Vector<T2, 3> const& v); Vec3 col(size_t i); template <typename T2> void setCol(size_t i, Vector<T2, 3> const& v); T determinant() const; Vec3 trace() const; Matrix3 inverse() const; bool isOrthogonal(T tolerance) const; void transpose(); void orthogonalize(); void invert(); // Apply the given 2d affine transformation to this matrix in global // coordinates void rotate(T angle, Vec2 const& point = Vec2()); void translate(Vec2 const& point); void scale(Vec2 const& scale, Vec2 const& point = Vec2()); void scale(T scale, Vec2 const& point = Vec2()); // Do an affine transformation of the given 2d vector. template <typename T2> Vector<T2, 2> transformVec2(Vector<T2, 2> const& v2) const; // The resulting angle of a transformation on any ray with this angle. float transformAngle(float angle) const; bool operator==(Matrix3 const& m2) const; bool operator!=(Matrix3 const& m2) const; Matrix3& operator*=(T const& s); Matrix3& operator/=(T const& s); Matrix3 operator*(T const& s) const; Matrix3 operator/(T const& s) const; Matrix3 operator-() const; template <typename T2> Matrix3& operator+=(Matrix3<T2> const& m2); template <typename T2> Matrix3& operator-=(Matrix3<T2> const& m2); template <typename T2> Matrix3& operator*=(Matrix3<T2> const& m2); template <typename T2> Matrix3 operator+(Matrix3<T2> const& m2) const; template <typename T2> Matrix3 operator-(Matrix3<T2> const& m2) const; template <typename T2> Matrix3 operator*(Matrix3<T2> const& m2) const; template <typename T2> Vec3 operator*(Vector<T2, 3> const& v) const; private: Rows m_rows; }; typedef Matrix3<float> Mat3F; typedef Matrix3<double> Mat3D; template <typename T> Matrix3<T> Matrix3<T>::identity() { return Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1); } template <typename T> Matrix3<T> Matrix3<T>::rotation(T angle, Vec2 const& point) { T s = sin(angle); T c = cos(angle); return Matrix3(c, -s, point[0] - c * point[0] + s * point[1], s, c, point[1] - s * point[0] - c * point[1], 0, 0, 1); } template <typename T> Matrix3<T> Matrix3<T>::translation(Vec2 const& point) { return Matrix3(1, 0, point[0], 0, 1, point[1], 0, 0, 1); } template <typename T> Matrix3<T> Matrix3<T>::scaling(T scale, Vec2 const& point) { return scaling(Vec2::filled(scale), point); } template <typename T> Matrix3<T> Matrix3<T>::scaling(Vec2 const& scale, Vec2 const& point) { return Matrix3(scale[0], 0, point[0] - point[0] * scale[0], 0, scale[1], point[1] - point[1] * scale[1], 0, 0, 1); } template <typename T> Matrix3<T>::Matrix3() {} template <typename T> Matrix3<T>::Matrix3(T r1c1, T r1c2, T r1c3, T r2c1, T r2c2, T r2c3, T r3c1, T r3c2, T r3c3) : m_rows(Vec3(r1c1, r1c2, r1c3), Vec3(r2c1, r2c2, r2c3), Vec3(r3c1, r3c2, r3c3)) {} template <typename T> Matrix3<T>::Matrix3(const Vec3& r1, const Vec3& r2, const Vec3& r3) : m_rows{r1, r2, r3} {} template <typename T> Matrix3<T>::Matrix3(T const* ptr) : m_rows{Vec3(ptr), Vec3(ptr + 3), Vec3(ptr + 6)} {} template <typename T> template <typename T2> Matrix3<T>::Matrix3(const Matrix3<T2>& m) { *this = m; } template <typename T> template <typename T2> Matrix3<T>& Matrix3<T>::operator=(const Matrix3<T2>& m) { m_rows = m.m_rows; return *this; } template <typename T> auto Matrix3<T>::operator[](const size_t i) -> Vec3 & { return m_rows[i]; } template <typename T> auto Matrix3<T>::operator[](const size_t i) const -> Vec3 const & { return m_rows[i]; } template <typename T> auto Matrix3<T>::ptr() -> EnableIfContiguousStorage<T*> { return m_rows[0].ptr(); } template <typename T> auto Matrix3<T>::ptr() const -> EnableIfContiguousStorage<T const*> { return m_rows[0].ptr(); } template <typename T> void Matrix3<T>::copy(T* loc) const { m_rows[0].copyFrom(loc); m_rows[1].copyFrom(loc + 3); m_rows[2].copyFrom(loc + 6); } template <typename T> auto Matrix3<T>::row(size_t i) const -> Vec3 { return operator[](i); } template <typename T> template <typename T2> void Matrix3<T>::setRow(size_t i, const Vector<T2, 3>& v) { operator[](i) = Vec3(v); } template <typename T> auto Matrix3<T>::col(size_t i) -> Vec3 { return Vec3(m_rows[0][i], m_rows[1][i], m_rows[2][i]); } template <typename T> template <typename T2> void Matrix3<T>::setCol(size_t i, const Vector<T2, 3>& v) { m_rows[0][i] = T(v[0]); m_rows[1][i] = T(v[1]); m_rows[2][i] = T(v[2]); } template <typename T> T Matrix3<T>::determinant() const { return m_rows[0][0] * m_rows[1][1] * m_rows[2][2] - m_rows[0][0] * m_rows[2][1] * m_rows[1][2] + m_rows[1][0] * m_rows[2][1] * m_rows[0][2] - m_rows[1][0] * m_rows[0][1] * m_rows[2][2] + m_rows[2][0] * m_rows[0][1] * m_rows[1][2] - m_rows[2][0] * m_rows[1][1] * m_rows[0][2]; } template <typename T> void Matrix3<T>::transpose() { std::swap(m_rows[1][0], m_rows[0][1]); std::swap(m_rows[2][0], m_rows[0][2]); std::swap(m_rows[2][1], m_rows[1][2]); } template <typename T> void Matrix3<T>::invert() { T d = determinant(); m_rows[0][0] = (m_rows[1][1] * m_rows[2][2] - m_rows[1][2] * m_rows[2][1]) / d; m_rows[0][1] = -(m_rows[0][1] * m_rows[2][2] - m_rows[0][2] * m_rows[2][1]) / d; m_rows[0][2] = (m_rows[0][1] * m_rows[1][2] - m_rows[0][2] * m_rows[1][1]) / d; m_rows[1][0] = -(m_rows[1][0] * m_rows[2][2] - m_rows[1][2] * m_rows[2][0]) / d; m_rows[1][1] = (m_rows[0][0] * m_rows[2][2] - m_rows[0][2] * m_rows[2][0]) / d; m_rows[1][2] = -(m_rows[0][0] * m_rows[1][2] - m_rows[0][2] * m_rows[1][0]) / d; m_rows[2][0] = (m_rows[1][0] * m_rows[2][1] - m_rows[1][1] * m_rows[2][0]) / d; m_rows[2][1] = -(m_rows[0][0] * m_rows[2][1] - m_rows[0][1] * m_rows[2][0]) / d; m_rows[2][2] = (m_rows[0][0] * m_rows[1][1] - m_rows[0][1] * m_rows[1][0]) / d; } template <typename T> Matrix3<T> Matrix3<T>::inverse() const { auto m = *this; m.invert(); return m; } template <typename T> void Matrix3<T>::orthogonalize() { m_rows[0].normalize(); T dot = m_rows[0] * m_rows[1]; m_rows[1][0] -= m_rows[0][0] * dot; m_rows[1][1] -= m_rows[0][1] * dot; m_rows[1][2] -= m_rows[0][2] * dot; m_rows[1].normalize(); dot = m_rows[1] * m_rows[2]; m_rows[2][0] -= m_rows[1][0] * dot; m_rows[2][1] -= m_rows[1][1] * dot; m_rows[2][2] -= m_rows[1][2] * dot; m_rows[2].normalize(); } template <typename T> bool Matrix3<T>::isOrthogonal(T tolerance) const { T det = determinant(); return std::fabs(det - 1) < tolerance || std::fabs(det + 1) < tolerance; } template <typename T> void Matrix3<T>::rotate(T angle, Vec2 const& point) { *this = rotation(angle, point) * *this; } template <typename T> void Matrix3<T>::translate(Vec2 const& point) { *this = translation(point) * *this; } template <typename T> void Matrix3<T>::scale(Vec2 const& scale, Vec2 const& point) { *this = scaling(scale, point) * *this; } template <typename T> void Matrix3<T>::scale(T scale, Vec2 const& point) { *this = scaling(scale, point) * *this; } template <typename T> template <typename T2> Vector<T2, 2> Matrix3<T>::transformVec2(Vector<T2, 2> const& point) const { Vector<T2, 3> res = (*this) * Vector<T2, 3>(point, 1); return res.vec2(); } template <typename T> float Matrix3<T>::transformAngle(float angle) const { Vec2 a = Vec2::withAngle(angle, 1.0f); Matrix3 m = *this; m[0][2] = 0; m[1][2] = 0; return m.transformVec2(a).angle(); } template <typename T> bool Matrix3<T>::operator==(Matrix3 const& m2) const { return tie(m_rows[0], m_rows[1], m_rows[2]) == tie(m2.m_rows[0], m2.m_rows[1], m2.m_rows[2]); } template <typename T> bool Matrix3<T>::operator!=(Matrix3 const& m2) const { return tie(m_rows[0], m_rows[1], m_rows[2]) != tie(m2.m_rows[0], m2.m_rows[1], m2.m_rows[2]); } template <typename T> Matrix3<T>& Matrix3<T>::operator*=(const T& s) { m_rows[0] *= s; m_rows[1] *= s; m_rows[2] *= s; return *this; } template <typename T> Matrix3<T>& Matrix3<T>::operator/=(const T& s) { m_rows[0] /= s; m_rows[1] /= s; m_rows[2] /= s; return *this; } template <typename T> auto Matrix3<T>::trace() const -> Vec3 { return Vec3(m_rows[0][0], m_rows[1][1], m_rows[2][2]); } template <typename T> Matrix3<T> Matrix3<T>::operator-() const { return Matrix3(-m_rows[0], -m_rows[1], -m_rows[2]); } template <typename T> template <typename T2> Matrix3<T>& Matrix3<T>::operator+=(const Matrix3<T2>& m) { m_rows[0] += m[0]; m_rows[1] += m[1]; m_rows[2] += m[2]; return *this; } template <typename T> template <typename T2> Matrix3<T>& Matrix3<T>::operator-=(const Matrix3<T2>& m) { m_rows[0] -= m[0]; m_rows[1] -= m[1]; m_rows[2] -= m[2]; return *this; } template <typename T> template <typename T2> Matrix3<T>& Matrix3<T>::operator*=(Matrix3<T2> const& m2) { *this = *this * m2; return *this; } template <typename T> template <typename T2> Matrix3<T> Matrix3<T>::operator+(const Matrix3<T2>& m2) const { return Matrix3<T>(m_rows[0] + m2[0], m_rows[1] + m2[1], m_rows[2] + m2[2]); } template <typename T> template <typename T2> Matrix3<T> Matrix3<T>::operator-(const Matrix3<T2>& m2) const { return Matrix3<T>(m_rows[0] - m2[0], m_rows[1] - m2[1], m_rows[2] - m2[2]); } template <typename T> template <typename T2> Matrix3<T> Matrix3<T>::operator*(const Matrix3<T2>& m2) const { return Matrix3<T>(m_rows[0][0] * m2[0][0] + m_rows[0][1] * m2[1][0] + m_rows[0][2] * m2[2][0], m_rows[0][0] * m2[0][1] + m_rows[0][1] * m2[1][1] + m_rows[0][2] * m2[2][1], m_rows[0][0] * m2[0][2] + m_rows[0][1] * m2[1][2] + m_rows[0][2] * m2[2][2], m_rows[1][0] * m2[0][0] + m_rows[1][1] * m2[1][0] + m_rows[1][2] * m2[2][0], m_rows[1][0] * m2[0][1] + m_rows[1][1] * m2[1][1] + m_rows[1][2] * m2[2][1], m_rows[1][0] * m2[0][2] + m_rows[1][1] * m2[1][2] + m_rows[1][2] * m2[2][2], m_rows[2][0] * m2[0][0] + m_rows[2][1] * m2[1][0] + m_rows[2][2] * m2[2][0], m_rows[2][0] * m2[0][1] + m_rows[2][1] * m2[1][1] + m_rows[2][2] * m2[2][1], m_rows[2][0] * m2[0][2] + m_rows[2][1] * m2[1][2] + m_rows[2][2] * m2[2][2]); } template <typename T> template <typename T2> auto Matrix3<T>::operator*(const Vector<T2, 3>& u) const -> Vec3 { return Vec3(m_rows[0][0] * u[0] + m_rows[0][1] * u[1] + m_rows[0][2] * u[2], m_rows[1][0] * u[0] + m_rows[1][1] * u[1] + m_rows[1][2] * u[2], m_rows[2][0] * u[0] + m_rows[2][1] * u[1] + m_rows[2][2] * u[2]); } template <typename T> Matrix3<T> Matrix3<T>::operator/(const T& s) const { return Matrix3<T>(m_rows[0] / s, m_rows[1] / s, m_rows[2] / s); } template <typename T> Matrix3<T> Matrix3<T>::operator*(const T& s) const { return Matrix3<T>(m_rows[0] * s, m_rows[1] * s, m_rows[2] * s); } template <typename T> T determinant(const Matrix3<T>& m) { return m.determinant(); } template <typename T> Matrix3<T> transpose(Matrix3<T> m) { return m.transpose(); } template <typename T> Matrix3<T> ortho(Matrix3<T> mat) { return mat.orthogonalize(); } template <typename T> Matrix3<T> operator*(T s, const Matrix3<T>& m) { return m * s; } template <typename T> std::ostream& operator<<(std::ostream& os, Matrix3<T> m) { os << m[0][0] << ' ' << m[0][1] << ' ' << m[0][2] << std::endl; os << m[1][0] << ' ' << m[1][1] << ' ' << m[1][2] << std::endl; os << m[2][0] << ' ' << m[2][1] << ' ' << m[2][2]; return os; } } template <typename T> struct fmt::formatter<Star::Matrix3<T>> : ostream_formatter {};