334 lines
8.4 KiB
C++
334 lines
8.4 KiB
C++
#pragma once
|
|
|
|
#include "dvc_config.h"
|
|
#include "dvc_pre_declare.h"
|
|
|
|
DV_CORE_BEGIN_NAMESPACE
|
|
|
|
template<typename T>
|
|
class TMatrix3
|
|
{
|
|
protected:
|
|
/* The matrix entries, indexed by [row][col].
|
|
| m[0][0] m[0][1] m[0][2] |
|
|
| m[1][0] m[1][1] m[1][2] |
|
|
| m[2][0] m[2][1] m[2][2] |
|
|
*/
|
|
union {
|
|
T m[3][3];
|
|
T _m[9];
|
|
};
|
|
|
|
public:
|
|
// constructor, It does NOT initialize the matrix for efficiency.
|
|
TMatrix3() {}
|
|
inline explicit TMatrix3(const T arr[3][3]) {
|
|
memcpy(m, arr, 9 * sizeof(T));
|
|
}
|
|
inline TMatrix3(const TMatrix3& rkMatrix) {
|
|
memcpy(m, rkMatrix.m, 9 * sizeof(T));
|
|
}
|
|
TMatrix3(
|
|
T m00, T m01, T m02,
|
|
T m10, T m11, T m12,
|
|
T m20, T m21, T m22)
|
|
{
|
|
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
|
|
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
|
|
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
|
|
}
|
|
|
|
public:
|
|
inline const T* operator[] (size_t iRow) const {
|
|
assert(iRow < 3);
|
|
return m[iRow];
|
|
}
|
|
|
|
inline T* operator[] (size_t iRow) {
|
|
assert(iRow < 3);
|
|
return m[iRow];
|
|
}
|
|
public:
|
|
// Operations
|
|
inline TMatrix3& operator = (const TMatrix3& rkMatrix) {
|
|
memcpy(m, rkMatrix.m, 9 * sizeof(T));
|
|
return *this;
|
|
}
|
|
|
|
bool operator == (const TMatrix3& rkMatrix) const;
|
|
inline bool operator != (const TMatrix3& rkMatrix) const {
|
|
return !operator==(rkMatrix);
|
|
}
|
|
|
|
// return this + rkMatrix
|
|
TMatrix3 operator + (const TMatrix3& rkMatrix) const;
|
|
|
|
// return this - Matrix
|
|
TMatrix3 operator - (const TMatrix3& rkMatrix) const;
|
|
|
|
// return this * rkMatrix
|
|
TMatrix3 operator * (const TMatrix3& rkMatrix) const;
|
|
|
|
// return -this
|
|
TMatrix3 operator - () const;
|
|
|
|
/* Matrix * Vector
|
|
| m[0][0] m[0][1] m[0][2] | |x|
|
|
| m[1][0] m[1][1] m[1][2] | * |y|
|
|
| m[2][0] m[2][1] m[2][2] | |z|
|
|
*/
|
|
TVector3<T> operator * (const TVector3<T>& rkVector) const;
|
|
|
|
/* Vector * Matrix
|
|
| m[0][0] m[0][1] m[0][2] |
|
|
[x, y, z] * | m[1][0] m[1][1] m[1][2] |
|
|
| m[2][0] m[2][1] m[2][2] |
|
|
*/
|
|
template<typename U>
|
|
friend TVector3<U> operator * (const TVector3<U>& rkVector, const TMatrix3<U>& rkMatrix);
|
|
|
|
/// Matrix * scalar
|
|
TMatrix3 operator * (T fScalar) const;
|
|
|
|
/// Scalar * matrix
|
|
template<typename U>
|
|
friend TMatrix3<U> operator * (U fScalar, const TMatrix3<U>& rkMatrix);
|
|
|
|
public:
|
|
static inline TMatrix3 makeTrans(T x, T y) {
|
|
return TMatrix3(
|
|
1, 0, x,
|
|
0, 1, y,
|
|
0, 0, 1);
|
|
}
|
|
|
|
static inline TMatrix3 makeTrans(const TVector2<T>& pos) {
|
|
return TMatrix3(
|
|
1, 0, pos.x,
|
|
0, 1, pos.y,
|
|
0, 0, 1);
|
|
}
|
|
|
|
static inline TMatrix3 makeScale(T x, T y) {
|
|
return TMatrix3(
|
|
x, 0, 0,
|
|
0, y, 0,
|
|
0, 0, 1);
|
|
}
|
|
|
|
static inline TMatrix3 makeScale(const TVector2<T>& scale) {
|
|
return TMatrix3(
|
|
scale.x, 0, 0,
|
|
0, scale.y, 0,
|
|
0, 0, 1);
|
|
}
|
|
|
|
//2D rotate, left multiplication
|
|
static inline TMatrix3 makeRotate(T angle) {
|
|
T s = sin(angle);
|
|
T c = cos(angle);
|
|
return TMatrix3(
|
|
c, -s, 0,
|
|
s, c, 0,
|
|
0, 0, 1);
|
|
}
|
|
|
|
public:
|
|
// Utilities
|
|
|
|
//return this^T
|
|
TMatrix3 transpose(void) const;
|
|
//rkInverse = this^{-1}?? return false mean no solution
|
|
bool inverse(TMatrix3& rkInverse, T fTolerance = 1e-06f) const;
|
|
//return this^{-1}, return ZERO if no solution
|
|
TMatrix3 inverse(T fTolerance = 1e-06f) const;
|
|
|
|
public:
|
|
static DV_CORE_API const TMatrix3 ZERO;
|
|
static DV_CORE_API const TMatrix3 IDENTITY;
|
|
};
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
bool TMatrix3<T>::operator== (const TMatrix3<T>& rkMatrix) const
|
|
{
|
|
for (size_t iRow = 0; iRow < 3; iRow++) {
|
|
for (size_t iCol = 0; iCol < 3; iCol++) {
|
|
if (m[iRow][iCol] != rkMatrix.m[iRow][iCol])
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
TMatrix3<T> TMatrix3<T>::operator + (const TMatrix3<T>& rkMatrix) const
|
|
{
|
|
TMatrix3<T> kSum;
|
|
for (size_t iRow = 0; iRow < 3; iRow++) {
|
|
for (size_t iCol = 0; iCol < 3; iCol++) {
|
|
kSum.m[iRow][iCol] = m[iRow][iCol] + rkMatrix.m[iRow][iCol];
|
|
}
|
|
}
|
|
return kSum;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
TMatrix3<T> TMatrix3<T>::operator - (const TMatrix3<T>& rkMatrix) const
|
|
{
|
|
TMatrix3<T> kDiff;
|
|
for (size_t iRow = 0; iRow < 3; iRow++) {
|
|
for (size_t iCol = 0; iCol < 3; iCol++) {
|
|
kDiff.m[iRow][iCol] = m[iRow][iCol] - rkMatrix.m[iRow][iCol];
|
|
}
|
|
}
|
|
return kDiff;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
TMatrix3<T> TMatrix3<T>::operator * (const TMatrix3<T>& rkMatrix) const
|
|
{
|
|
TMatrix3<T> kProd;
|
|
for (size_t iRow = 0; iRow < 3; iRow++) {
|
|
for (size_t iCol = 0; iCol < 3; iCol++) {
|
|
kProd.m[iRow][iCol] =
|
|
m[iRow][0] * rkMatrix.m[0][iCol] +
|
|
m[iRow][1] * rkMatrix.m[1][iCol] +
|
|
m[iRow][2] * rkMatrix.m[2][iCol];
|
|
}
|
|
}
|
|
return kProd;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
TMatrix3<T> TMatrix3<T>::operator - () const
|
|
{
|
|
TMatrix3<T> kNeg;
|
|
for (size_t i = 0; i < 9; i++) {
|
|
kNeg._m[i] = -_m[i];
|
|
}
|
|
return kNeg;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
TVector3<T> TMatrix3<T>::operator* (const TVector3<T>& rkPoint) const
|
|
{
|
|
TVector3<T> kProd(
|
|
m[0][0] * rkPoint[0] + m[0][1] * rkPoint[1] + m[0][2] * rkPoint[2],
|
|
m[1][0] * rkPoint[0] + m[1][1] * rkPoint[1] + m[1][2] * rkPoint[2],
|
|
m[2][0] * rkPoint[0] + m[2][1] * rkPoint[1] + m[2][2] * rkPoint[2]);
|
|
|
|
return kProd;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
inline TVector3<T> operator* (const TVector3<T>& rkPoint, const TMatrix3<T>& rkMatrix)
|
|
{
|
|
TVector3<T> kProd(
|
|
rkPoint[0] * rkMatrix.m[0][0] + rkPoint[1] * rkMatrix.m[1][0] + rkPoint[2] * rkMatrix.m[2][0],
|
|
rkPoint[0] * rkMatrix.m[0][1] + rkPoint[1] * rkMatrix.m[1][1] + rkPoint[2] * rkMatrix.m[2][1],
|
|
rkPoint[0] * rkMatrix.m[0][2] + rkPoint[1] * rkMatrix.m[1][2] + rkPoint[2] * rkMatrix.m[2][2]);
|
|
|
|
return kProd;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
TMatrix3<T> TMatrix3<T>::operator* (T fScalar) const
|
|
{
|
|
TMatrix3<T> kProd;
|
|
for (size_t iRow = 0; iRow < 3; iRow++) {
|
|
for (size_t iCol = 0; iCol < 3; iCol++)
|
|
kProd[iRow][iCol] = fScalar * m[iRow][iCol];
|
|
}
|
|
return kProd;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
TMatrix3<T> operator* (T fScalar, const TMatrix3<T>& rkMatrix)
|
|
{
|
|
TMatrix3<T> kProd;
|
|
for (size_t iRow = 0; iRow < 3; iRow++) {
|
|
for (size_t iCol = 0; iCol < 3; iCol++)
|
|
kProd[iRow][iCol] = fScalar * rkMatrix.m[iRow][iCol];
|
|
}
|
|
return kProd;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
TMatrix3<T> TMatrix3<T>::transpose(void) const
|
|
{
|
|
TMatrix3<T> kTranspose;
|
|
for (size_t iRow = 0; iRow < 3; iRow++) {
|
|
for (size_t iCol = 0; iCol < 3; iCol++)
|
|
kTranspose[iRow][iCol] = m[iCol][iRow];
|
|
}
|
|
return kTranspose;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
bool TMatrix3<T>::inverse(TMatrix3<T>& rkInverse, T fTolerance) const
|
|
{
|
|
// Invert a 3x3 using cofactors. This is about 8 times faster than
|
|
// the Numerical Recipes code which uses Gaussian elimination.
|
|
|
|
rkInverse[0][0] = m[1][1] * m[2][2] -
|
|
m[1][2] * m[2][1];
|
|
rkInverse[0][1] = m[0][2] * m[2][1] -
|
|
m[0][1] * m[2][2];
|
|
rkInverse[0][2] = m[0][1] * m[1][2] -
|
|
m[0][2] * m[1][1];
|
|
rkInverse[1][0] = m[1][2] * m[2][0] -
|
|
m[1][0] * m[2][2];
|
|
rkInverse[1][1] = m[0][0] * m[2][2] -
|
|
m[0][2] * m[2][0];
|
|
rkInverse[1][2] = m[0][2] * m[1][0] -
|
|
m[0][0] * m[1][2];
|
|
rkInverse[2][0] = m[1][0] * m[2][1] -
|
|
m[1][1] * m[2][0];
|
|
rkInverse[2][1] = m[0][1] * m[2][0] -
|
|
m[0][0] * m[2][1];
|
|
rkInverse[2][2] = m[0][0] * m[1][1] -
|
|
m[0][1] * m[1][0];
|
|
|
|
T fDet =
|
|
m[0][0] * rkInverse[0][0] +
|
|
m[0][1] * rkInverse[1][0] +
|
|
m[0][2] * rkInverse[2][0];
|
|
|
|
if (std::abs(fDet) <= fTolerance)
|
|
return false;
|
|
|
|
T fInvDet = T(1.0) / fDet;
|
|
for (size_t iRow = 0; iRow < 3; iRow++) {
|
|
for (size_t iCol = 0; iCol < 3; iCol++)
|
|
rkInverse[iRow][iCol] *= fInvDet;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
template<typename T>
|
|
TMatrix3<T> TMatrix3<T>::inverse(T fTolerance) const
|
|
{
|
|
TMatrix3<T> kInverse = TMatrix3<T>::ZERO;
|
|
inverse(kInverse, fTolerance);
|
|
return kInverse;
|
|
}
|
|
|
|
typedef TMatrix3<float32_t> fMatrix3;
|
|
|
|
DV_CORE_END_NAMESPACE
|
|
|