550 lines
12 KiB
C++
550 lines
12 KiB
C++
#pragma once
|
|
|
|
#include "Core/public/Math/Boilerplate.h"
|
|
|
|
#include "Core/public/Math/MathAbstractTypes.h"
|
|
#include "Core/public/Math/MathFwd.h"
|
|
#include "Core/public/Math/Vector3.hpp"
|
|
|
|
#ifndef MATRIX3_H
|
|
#define MATRIX3_H
|
|
|
|
namespace Phanes::Core::Math {
|
|
|
|
|
|
// 3x3 Matrix defined in column-major order.
|
|
// Accessed by M[Row][Col].
|
|
|
|
|
|
template<RealType T, bool S>
|
|
struct TMatrix3
|
|
{
|
|
public:
|
|
|
|
union
|
|
{
|
|
struct
|
|
{
|
|
/// <summary>
|
|
/// Column one.
|
|
/// </summary>
|
|
TVector3<T, S> c0;
|
|
|
|
/// <summary>
|
|
/// Column two.
|
|
/// </summary>
|
|
TVector3<T, S> c1;
|
|
|
|
/// <summary>
|
|
/// Column three.
|
|
/// </summary>
|
|
TVector3<T, S> c2;
|
|
};
|
|
|
|
T data[3][4];
|
|
};
|
|
|
|
|
|
|
|
public:
|
|
|
|
TMatrix3() = default;
|
|
|
|
TMatrix3(TMatrix3<T, S>&& m)
|
|
: c0(std::move(m.c0)), c1(std::move(m.c1)), c2(std::move(m.c2))
|
|
{};
|
|
|
|
TMatrix3(const TMatrix3<T, S>& m)
|
|
: c0(m.c0), c1(m.c1), c2(m.c2)
|
|
{};
|
|
|
|
/**
|
|
* Construct Matrix from 2d array.
|
|
*
|
|
* @param(fields) 2D Array with row major order.
|
|
*/
|
|
|
|
TMatrix3(T fields[3][3])
|
|
{
|
|
this->c0 = TVector3<T, S>(fields[0][0], fields[1][0], fields[2][0]);
|
|
this->c1 = TVector3<T, S>(fields[0][1], fields[1][1], fields[2][1]);
|
|
this->c2 = TVector3<T, S>(fields[0][2], fields[1][2], fields[2][2]);
|
|
}
|
|
|
|
/**
|
|
* Construct Matrix from parameters.
|
|
*
|
|
* @param(n00) M[0][0]
|
|
* @param(n10) M[0][1]
|
|
* @param(n20) M[0][2]
|
|
* @param(n01) M[1][0]
|
|
* ...
|
|
*
|
|
* @note nXY = n[Row][Col]
|
|
*/
|
|
|
|
TMatrix3(T n00, T n01, T n02,
|
|
T n10, T n11, T n12,
|
|
T n20, T n21, T n22)
|
|
{
|
|
this->c0 = TVector3<T, S>(n00,n10,n20);
|
|
this->c1 = TVector3<T, S>(n01,n11,n21);
|
|
this->c2 = TVector3<T, S>(n02,n12,n22);
|
|
}
|
|
|
|
/**
|
|
* Construct Matrix from two 2d vector columns.
|
|
*
|
|
* @param(v1) Column zero
|
|
* @param(v2) Column one
|
|
*/
|
|
|
|
TMatrix3(const TVector3<T, S>& v1, const TVector3<T, S>& v2, const TVector3<T, S> v3)
|
|
{
|
|
this->c0 = v1;
|
|
this->c1 = v2;
|
|
this->c2 = v3;
|
|
}
|
|
|
|
TMatrix3<T, S>& operator= (TMatrix3<T, S>&& m)
|
|
{
|
|
if (this != &m)
|
|
{
|
|
this->c0 = std::move(m.c0);
|
|
this->c1 = std::move(m.c1);
|
|
this->c2 = std::move(m.c2);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
TMatrix3<T, S>& operator= (const TMatrix3<T, S>& m)
|
|
{
|
|
if (this != &m)
|
|
{
|
|
this->c0 = m.c0;
|
|
this->c1 = m.c1;
|
|
this->c2 = m.c2;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
public:
|
|
|
|
FORCEINLINE T operator() (int n, int m) const
|
|
{
|
|
return this->data[m][n];
|
|
}
|
|
|
|
FORCEINLINE T& operator() (int n, int m)
|
|
{
|
|
return this->data[m][n];
|
|
}
|
|
|
|
FORCEINLINE TVector3<T, S>& operator[] (int m)
|
|
{
|
|
return (*reinterpret_cast<TVector3<T, S>*>(this->data[m]));
|
|
}
|
|
|
|
FORCEINLINE const TVector3<T, S> operator[] (int m) const
|
|
{
|
|
return (*reinterpret_cast<TVector3<T, S>*>(this->data[m]));
|
|
}
|
|
|
|
};
|
|
|
|
|
|
// ==================== //
|
|
// Matrix3 operator //
|
|
// ==================== //
|
|
|
|
/**
|
|
* Adds scalar to matrix componentwise
|
|
*
|
|
* @param(m) Matrix
|
|
* @param(s) Scalar
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S>& operator+= (TMatrix3<T, S>& m1, T s)
|
|
{
|
|
m1.c0 += s;
|
|
m1.c1 += s;
|
|
m1.c2 += s;
|
|
|
|
return m1;
|
|
}
|
|
|
|
/**
|
|
* Adds matrix to matrix componentwise
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(m2) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S>& operator+= (TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
|
|
{
|
|
m1.c0 += m2.c0;
|
|
m1.c1 += m2.c1;
|
|
m1.c2 += m2.c2;
|
|
|
|
return m1;
|
|
}
|
|
|
|
/**
|
|
* Substract scalar from matrix componentwise
|
|
*
|
|
* @param(m) Matrix
|
|
* @param(s) Scalar
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S>& operator-= (TMatrix3<T, S>& m1, T s)
|
|
{
|
|
m1.c0 -= s;
|
|
m1.c1 -= s;
|
|
m1.c2 -= s;
|
|
|
|
return m1;
|
|
}
|
|
|
|
/**
|
|
* Substract matrix from matrix componentwise
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(m2) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S>& operator-= (TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
|
|
{
|
|
m1.c0 -= m2.c0;
|
|
m1.c1 -= m2.c1;
|
|
m1.c2 -= m2.c2;
|
|
|
|
return m1;
|
|
}
|
|
|
|
/**
|
|
* Multiply matrix with scalar
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(s) Scalar
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S>& operator*= (TMatrix3<T, S>& m1, T s)
|
|
{
|
|
m1.c0 *= s;
|
|
m1.c1 *= s;
|
|
m1.c2 *= s;
|
|
|
|
return m1;
|
|
}
|
|
|
|
/**
|
|
* Matrix on matrix (componentwise)
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(m2) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S>& operator*= (TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2);
|
|
|
|
/**
|
|
* Multiply matrix with scalar
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(s) Scalar
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S>& operator/= (TMatrix3<T, S>& m1, T s)
|
|
{
|
|
s = (T)1.0 / s;
|
|
m1.c0 *= s;
|
|
m1.c1 *= s;
|
|
m1.c2 *= s;
|
|
|
|
return m1;
|
|
}
|
|
|
|
/**
|
|
* Matrix on matrix (componentwise)
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(m2) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S>& operator/= (TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
|
|
{
|
|
m1.c0 /= m2.c0;
|
|
m1.c1 /= m2.c1;
|
|
m1.c2 /= m2.c2;
|
|
|
|
return m1;
|
|
}
|
|
|
|
/**
|
|
* Add scalar to matrix componentwise
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(s) Scalar
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S> operator+ (const TMatrix3<T, S>& m, T s)
|
|
{
|
|
return TMatrix3<T, S>(m.c0 + s,
|
|
m.c1 + s,
|
|
m.c2 + s);
|
|
}
|
|
|
|
/**
|
|
* Add matrix to matrix componentwise
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(m2) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S> operator+ (const TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
|
|
{
|
|
return TMatrix3<T, S>(m1.c0 + m2.c0,
|
|
m1.c1 + m2.c1,
|
|
m1.c2 + m2.c2);
|
|
}
|
|
|
|
/**
|
|
* Add scalar to matrix componentwise
|
|
*
|
|
* @param(m) Matrix
|
|
* @param(s) Scalar
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S> operator- (const TMatrix3<T, S>& m, T s)
|
|
{
|
|
return TMatrix3<T, S>(m.c0 - s,
|
|
m.c1 - s,
|
|
m.c2 - s);
|
|
}
|
|
|
|
/**
|
|
* Add scalar to matrix componentwise
|
|
*
|
|
* @param(m) Matrix
|
|
* @param(s) Scalar
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S> operator- (const TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
|
|
{
|
|
return TMatrix3<T, S>(m1.c0 - m2.c0,
|
|
m1.c1 - m2.c1,
|
|
m1.c2 - m2.c2);
|
|
}
|
|
|
|
/**
|
|
* Multiply scalar with matrix
|
|
*
|
|
* @param(m) Matrix
|
|
* @param(s) Scalar
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S> operator* (const TMatrix3<T, S>& m, float s)
|
|
{
|
|
return TMatrix3<T, S>(m.c0 * s,
|
|
m.c1 * s,
|
|
m.c2 * s);
|
|
}
|
|
|
|
/**
|
|
* Multiply scalar with matrix
|
|
*
|
|
* @param(m) Matrix
|
|
* @param(s) Scalar
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S> operator/ (const TMatrix3<T, S>& m, float s)
|
|
{
|
|
s = (T)1.0 / s;
|
|
return TMatrix3<T, S>(m.c0 * s,
|
|
m.c1 * s,
|
|
m.c2 * s);
|
|
}
|
|
|
|
/**
|
|
* Multiplay matrix by matrix (componentwise)
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(m2) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S> operator* (const TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2);
|
|
|
|
template<RealType T, bool S>
|
|
TVector3<T, S> operator* (const TMatrix3<T, S>& m1, const TVector3<T, S>& v);
|
|
|
|
/**
|
|
* Compare matrix with other matrix.
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(m2) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
bool operator== (const TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
|
|
{
|
|
return (m1.c0 == m2.c0 && m1.c1 == m2.c1 && m1.c2 == m2.c2);
|
|
}
|
|
|
|
/**
|
|
* Compare matrix with other matrix.
|
|
*
|
|
* @param(m1) Matrix
|
|
* @param(m2) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
bool operator!= (const TMatrix3<T, S>& m1, const TMatrix3<T, S>& m2)
|
|
{
|
|
return (m1.c0 != m2.c0 || m1.c1 != m2.c1 || m1.c2 != m2.c2);
|
|
}
|
|
|
|
|
|
// =============================== //
|
|
// Matrix function definition //
|
|
// =============================== //
|
|
|
|
/**
|
|
* Calculate determinant of 3x3 Matrix
|
|
*
|
|
* @param(m1) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
T Determinant(const TMatrix3<T, S>& m1)
|
|
{
|
|
return m1(0, 0) * (m1(1, 1) * m1(2, 2) - m1(1, 2) * m1(2, 1))
|
|
- m1(0, 1) * (m1(1, 0) * m1(2, 2) - m1(1, 2) * m1(2, 0))
|
|
+ m1(0, 2) * (m1(1, 0) * m1(2, 1) - m1(1, 1) * m1(2, 0));
|
|
}
|
|
|
|
/**
|
|
* Calculate inverse of 3x3 Matrix
|
|
*
|
|
* @see [FUNC]Inverse
|
|
*
|
|
* @param(m1) Matrix
|
|
*
|
|
* @note Stores result in m1.
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
bool InverseV(TMatrix3<T, S>& m1)
|
|
{
|
|
// Rows of inversed matrix
|
|
TVector3<T, S> r0 = CrossP(m1.c1, m1.c2);
|
|
TVector3<T, S> r1 = CrossP(m1.c2, m1.c0);
|
|
TVector3<T, S> r2 = CrossP(m1.c0, m1.c1);
|
|
|
|
T _1_det = (T)1.0 / Determinant(m1);
|
|
|
|
if (_1_det == (T)0.0)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Store matrix transposed
|
|
m1 = TMatrix3<T, S>(r0.x, r0.y, r0.z,
|
|
r1.x, r1.y, r1.z,
|
|
r2.x, r2.y, r2.z);
|
|
|
|
m1 *= _1_det;
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Get transpose of matrix.
|
|
*
|
|
* @param(m1) Matrix
|
|
*
|
|
* @note Result is stored in m1;
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S>& TransposeV(TMatrix3<T, S>& m1);
|
|
|
|
|
|
// =============== //
|
|
// WITH RETURN //
|
|
// =============== //
|
|
|
|
/**
|
|
* Calculate inverse of 3x3 Matrix
|
|
*
|
|
* @param(m1) Matrix
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
bool Inverse(const TMatrix3<T, S>& m1, Ref<TMatrix3<T, S>> r)
|
|
{
|
|
// r is a row-major matrix.
|
|
TMatrix3<T, S> row;
|
|
row[0] = CrossP(m1.c1, m1.c2);
|
|
row[1] = CrossP(m1.c2, m1.c0);
|
|
row[2] = CrossP(m1.c0, m1.c1);
|
|
|
|
T _1_det = (T)1.0 / Determinant(m1);
|
|
|
|
if (Phanes::Core::Math::Equals(_1_det, (T)0.0))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
row *= _1_det;
|
|
|
|
// Store matrix transposed.
|
|
(*r).data[0][0] = row.data[0][0]; (*r).data[1][0] = row.data[0][1]; (*r).data[2][0] = row.data[0][2];
|
|
(*r).data[0][1] = row.data[1][0]; (*r).data[1][1] = row.data[1][1]; (*r).data[2][1] = row.data[1][2];
|
|
(*r).data[0][2] = row.data[2][0]; (*r).data[1][2] = row.data[2][1]; (*r).data[2][2] = row.data[2][2];
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Get transpose of matrix.
|
|
*
|
|
* @param(m1) Matrix
|
|
*
|
|
* @note Result is stored in m1;
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
TMatrix3<T, S> Transpose(const TMatrix3<T, S>& m1);
|
|
|
|
/**
|
|
* Checks if matrix is an identity matrix.
|
|
*/
|
|
|
|
template<RealType T, bool S>
|
|
bool IsIdentityMatrix(const TMatrix3<T, S>& m1)
|
|
{
|
|
return (abs(m1(0, 0) - (T)1.0) < P_FLT_INAC && abs(m1(0, 1)) < P_FLT_INAC && abs(m1(0, 2)) < P_FLT_INAC &&
|
|
abs(m1(1, 0)) < P_FLT_INAC && abs(m1(1, 1) - (T)1.0) < P_FLT_INAC && abs(m1(1, 2)) < P_FLT_INAC &&
|
|
abs(m1(2, 0)) < P_FLT_INAC && abs(m1(2, 1)) < P_FLT_INAC && abs(m1(2, 2) - (T)1.0) < P_FLT_INAC);
|
|
}
|
|
|
|
} // Phanes::Core::Math
|
|
|
|
|
|
#endif // !MATRIX3_H
|
|
|
|
#include "Core/public/Math/Matrix3.inl" |