osb/source/core/StarMatrix3.hpp

457 lines
12 KiB
C++
Raw Normal View History

2023-06-20 04:33:09 +00:00
#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;
}
}
#endif