1 files changed, 456 insertions, 0 deletions
diff --git a/source/core/StarMatrix3.hpp b/source/core/StarMatrix3.hpp
new file mode 100644
index 0000000..6c688c8
--- /dev/null
+++ b/source/core/StarMatrix3.hpp
@@ -0,0 +1,456 @@
+#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