460 lines
13 KiB
C++
460 lines
13 KiB
C++
#ifndef STAR_MATRIX3_HPP
|
|
#define STAR_MATRIX3_HPP
|
|
|
|
#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 {};
|
|
|
|
#endif
|