You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
334 lines
11 KiB
334 lines
11 KiB
#ifndef TINY_MATRIX_H
|
|
#define TINY_MATRIX_H
|
|
|
|
#include "TinyVector.h"
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
class ColumnMatrix3
|
|
{
|
|
private:
|
|
TinyVector<T, 3> V_[3];
|
|
//////////////////////////////////////////////////////////////////////////
|
|
public:
|
|
ColumnMatrix3()
|
|
{
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3(const TinyVector<T, 3> &T0, const TinyVector<T, 3> &T1, const TinyVector<T, 3> &T2)
|
|
{
|
|
V_[0] = T0, V_[1] = T1, V_[2] = T2;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3(const T *T0, const T *T1, const T *T2)
|
|
{
|
|
V_[0] = T0, V_[1] = T1, V_[2] = T2;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3(const T *V)
|
|
{
|
|
V_[0] = &V[0];
|
|
V_[1] = &V[1];
|
|
V_[2] = &V[2];
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3(const ColumnMatrix3<T> &CM)
|
|
{
|
|
V_[0] = CM[0], V_[1] = CM[1], V_[2] = CM[2];
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3(const T a0, const T a1, const T a2, const T a3, const T a4, const T a5,
|
|
const T a6, const T a7, const T a8, bool columnMajor = true)
|
|
{
|
|
SetEntries(a0, a1, a2, a3, a4, a5, a6, a7, a8, columnMajor);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
void SetEntries(const T a0, const T a1, const T a2, const T a3, const T a4, const T a5,
|
|
const T a6, const T a7, const T a8, bool columnMajor = true)
|
|
{
|
|
if (columnMajor)
|
|
{
|
|
V_[0][0] = a0;
|
|
V_[0][1] = a1;
|
|
V_[0][2] = a2;
|
|
V_[1][0] = a3;
|
|
V_[1][1] = a4;
|
|
V_[1][2] = a5;
|
|
V_[2][0] = a6;
|
|
V_[2][1] = a7;
|
|
V_[2][2] = a8;
|
|
}
|
|
else
|
|
{
|
|
V_[0][0] = a0;
|
|
V_[0][1] = a3;
|
|
V_[0][2] = a6;
|
|
V_[1][0] = a1;
|
|
V_[1][1] = a4;
|
|
V_[1][2] = a7;
|
|
V_[2][0] = a2;
|
|
V_[2][1] = a5;
|
|
V_[2][2] = a8;
|
|
}
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
TinyVector<T, 3> operator*(const TinyVector<T, 3> &R) const
|
|
{
|
|
return TinyVector<T, 3>(
|
|
V_[0][0] * R[0] + V_[1][0] * R[1] + V_[2][0] * R[2],
|
|
V_[0][1] * R[0] + V_[1][1] * R[1] + V_[2][1] * R[2],
|
|
V_[0][2] * R[0] + V_[1][2] * R[1] + V_[2][2] * R[2]);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> operator*(const ColumnMatrix3<T> &R) const
|
|
{
|
|
return ColumnMatrix3<T>(
|
|
V_[0][0] * R[0][0] + V_[1][0] * R[0][1] + V_[2][0] * R[0][2],
|
|
V_[0][1] * R[0][0] + V_[1][1] * R[0][1] + V_[2][1] * R[0][2],
|
|
V_[0][2] * R[0][0] + V_[1][2] * R[0][1] + V_[2][2] * R[0][2],
|
|
|
|
V_[0][0] * R[1][0] + V_[1][0] * R[1][1] + V_[2][0] * R[1][2],
|
|
V_[0][1] * R[1][0] + V_[1][1] * R[1][1] + V_[2][1] * R[1][2],
|
|
V_[0][2] * R[1][0] + V_[1][2] * R[1][1] + V_[2][2] * R[1][2],
|
|
|
|
V_[0][0] * R[2][0] + V_[1][0] * R[2][1] + V_[2][0] * R[2][2],
|
|
V_[0][1] * R[2][0] + V_[1][1] * R[2][1] + V_[2][1] * R[2][2],
|
|
V_[0][2] * R[2][0] + V_[1][2] * R[2][1] + V_[2][2] * R[2][2]
|
|
);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> TransposeTimes(const ColumnMatrix3<T> &R) const
|
|
{
|
|
return ColumnMatrix3<T>(
|
|
V_[0].Dot(R[0]), V_[1].Dot(R[0]), V_[2].Dot(R[0]),
|
|
V_[0].Dot(R[1]), V_[1].Dot(R[1]), V_[2].Dot(R[1]),
|
|
V_[0].Dot(R[2]), V_[1].Dot(R[2]), V_[2].Dot(R[2])
|
|
);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> TimesTranspose(const ColumnMatrix3<T> &R) const
|
|
{
|
|
return ColumnMatrix3<T>(
|
|
V_[0][0] * R[0][0] + V_[1][0] * R[1][0] + V_[2][0] * R[2][0],
|
|
V_[0][1] * R[0][0] + V_[1][1] * R[1][0] + V_[2][1] * R[2][0],
|
|
V_[0][2] * R[0][0] + V_[1][2] * R[1][0] + V_[2][2] * R[2][0],
|
|
|
|
V_[0][0] * R[0][1] + V_[1][0] * R[1][1] + V_[2][0] * R[2][1],
|
|
V_[0][1] * R[0][1] + V_[1][1] * R[1][1] + V_[2][1] * R[2][1],
|
|
V_[0][2] * R[0][1] + V_[1][2] * R[1][1] + V_[2][2] * R[2][1],
|
|
|
|
V_[0][0] * R[0][2] + V_[1][0] * R[1][2] + V_[2][0] * R[2][2],
|
|
V_[0][1] * R[0][2] + V_[1][1] * R[1][2] + V_[2][1] * R[2][2],
|
|
V_[0][2] * R[0][2] + V_[1][2] * R[1][2] + V_[2][2] * R[2][2]
|
|
);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> TransposeTimesTranspose(const ColumnMatrix3<T> &R) const
|
|
{
|
|
return ColumnMatrix3<T>(
|
|
R[0][0] * V_[0][0] + R[1][0] * V_[0][1] + R[2][0] * V_[0][2],
|
|
R[0][1] * V_[0][0] + R[1][1] * V_[0][1] + R[2][1] * V_[0][2],
|
|
R[0][2] * V_[0][0] + R[1][2] * V_[0][1] + R[2][2] * V_[0][2],
|
|
|
|
R[0][0] * V_[1][0] + R[1][0] * V_[1][1] + R[2][0] * V_[1][2],
|
|
R[0][1] * V_[1][0] + R[1][1] * V_[1][1] + R[2][1] * V_[1][2],
|
|
R[0][2] * V_[1][0] + R[1][2] * V_[1][1] + R[2][2] * V_[1][2],
|
|
|
|
R[0][0] * V_[2][0] + R[1][0] * V_[2][1] + R[2][0] * V_[2][2],
|
|
R[0][1] * V_[2][0] + R[1][1] * V_[2][1] + R[2][1] * V_[2][2],
|
|
R[0][2] * V_[2][0] + R[1][2] * V_[2][1] + R[2][2] * V_[2][2],
|
|
false
|
|
);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> TimesDiagonal(const TinyVector<T, 3> &R) const
|
|
{
|
|
return ColumnMatrix3<T>(R[0] * V_[0], R[1] * V_[1], R[2] * V_[2]);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> DiagonalTimes(const TinyVector<T, 3> &R) const
|
|
{
|
|
return ColumnMatrix3<T>(
|
|
R[0] * V_[0][0], R[1] * V_[0][1], R[2] * V_[0][2],
|
|
R[0] * V_[1][0], R[1] * V_[1][1], R[2] * V_[1][2],
|
|
R[0] * V_[2][0], R[1] * V_[2][1], R[2] * V_[2][2]
|
|
);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
const TinyVector<T, 3> &operator[](int i) const
|
|
{
|
|
return V_[i];
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
TinyVector<T, 3> &operator[](int i)
|
|
{
|
|
return V_[i];
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> Inverse() const
|
|
{
|
|
ColumnMatrix3<T> MI;
|
|
|
|
MI[0][0] = V_[1][1] * V_[2][2] - V_[2][1] * V_[1][2];
|
|
MI[0][1] = V_[2][1] * V_[0][2] - V_[0][1] * V_[2][2];
|
|
MI[0][2] = V_[0][1] * V_[1][2] - V_[1][1] * V_[0][2];
|
|
|
|
MI[1][0] = V_[2][0] * V_[1][2] - V_[1][0] * V_[2][2];
|
|
MI[1][1] = V_[0][0] * V_[2][2] - V_[2][0] * V_[0][2];
|
|
MI[1][2] = V_[1][0] * V_[0][2] - V_[0][0] * V_[1][2];
|
|
|
|
MI[2][0] = V_[1][0] * V_[2][1] - V_[2][0] * V_[1][1];
|
|
MI[2][1] = V_[2][0] * V_[0][1] - V_[0][0] * V_[2][1];
|
|
MI[2][2] = V_[0][0] * V_[1][1] - V_[1][0] * V_[0][1];
|
|
|
|
T det = V_[0][0] * MI[0][0] + V_[1][0] * MI[0][1] + V_[2][0] * MI[0][2];
|
|
if (fabs(det) > 1.0e-16)
|
|
{
|
|
MI[0] /= det;
|
|
MI[1] /= det;
|
|
MI[2] /= det;
|
|
}
|
|
return MI;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
T Determinant () const
|
|
{
|
|
return V_[0][0] * (V_[1][1] * V_[2][2] - V_[2][1] * V_[1][2]) + V_[1][0] * (V_[2][1] * V_[0][2] - V_[0][1] * V_[2][2]) + V_[2][0] * (V_[0][1] * V_[1][2] - V_[1][1] * V_[0][2]);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> &operator= (const ColumnMatrix3<T> &CM)
|
|
{
|
|
V_[0] = CM[0], V_[1] = CM[1], V_[2] = CM[2];
|
|
return *this;
|
|
}
|
|
////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> &MakeZero()
|
|
{
|
|
V_[0].reset_zero();
|
|
V_[1].reset_zero();
|
|
V_[2].reset_zero();
|
|
return *this;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> &MakeIdentity()
|
|
{
|
|
V_[0][0] = (T)1, V_[0][1] = (T)0, V_[0][2] = (T)0;
|
|
V_[1][0] = (T)0, V_[1][1] = (T)1, V_[1][2] = (T)0;
|
|
V_[2][0] = (T)0, V_[2][1] = (T)0, V_[2][2] = (T)1;
|
|
return *this;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> operator+ (const ColumnMatrix3<T> &mat) const
|
|
{
|
|
return ColumnMatrix3<T>
|
|
(
|
|
V_[0][0] + mat[0][0], V_[0][1] + mat[0][1], V_[0][2] + mat[0][2],
|
|
V_[1][0] + mat[1][0], V_[1][1] + mat[1][1], V_[1][2] + mat[1][2],
|
|
V_[2][0] + mat[2][0], V_[2][1] + mat[2][1], V_[2][2] + mat[2][2]
|
|
);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> operator- (const ColumnMatrix3<T> &mat) const
|
|
{
|
|
return ColumnMatrix3<T>
|
|
(
|
|
V_[0][0] - mat[0][0], V_[0][1] - mat[0][1], V_[0][2] - mat[0][2],
|
|
V_[1][0] - mat[1][0], V_[1][1] - mat[1][1], V_[1][2] - mat[1][2],
|
|
V_[2][0] - mat[2][0], V_[2][1] - mat[2][1], V_[2][2] - mat[2][2]
|
|
);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> operator* (const T scalar) const
|
|
{
|
|
return ColumnMatrix3<T>
|
|
(
|
|
scalar * V_[0][0], scalar * V_[0][1], scalar * V_[0][2],
|
|
scalar * V_[1][0], scalar * V_[1][1], scalar * V_[1][2],
|
|
scalar * V_[2][0], scalar * V_[2][1], scalar * V_[2][2]
|
|
);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> operator/ (const T scalar) const
|
|
{
|
|
if (scalar != (T)0)
|
|
{
|
|
T inv_scalar = ((T)1) / scalar;
|
|
return ColumnMatrix3<T>
|
|
(
|
|
inv_scalar * V_[0][0], inv_scalar * V_[0][1], inv_scalar * V_[0][2],
|
|
inv_scalar * V_[1][0], inv_scalar * V_[1][1], inv_scalar * V_[1][2],
|
|
inv_scalar * V_[2][0], inv_scalar * V_[2][1], inv_scalar * V_[2][2]
|
|
);
|
|
}
|
|
else
|
|
{
|
|
return ColumnMatrix3<T>();
|
|
}
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> operator- () const
|
|
{
|
|
return ColumnMatrix3<T>
|
|
(
|
|
-V_[0][0], -V_[0][1], -V_[0][2],
|
|
-V_[1][0], -V_[1][1], -V_[1][2],
|
|
-V_[2][0], -V_[2][1], -V_[2][2]
|
|
);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> &operator+= (const ColumnMatrix3<T> &mat)
|
|
{
|
|
V_[0] += mat[0];
|
|
V_[1] += mat[1];
|
|
V_[2] += mat[2];
|
|
return *this;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> &operator-= (const ColumnMatrix3<T> &mat)
|
|
{
|
|
V_[0] -= mat[0];
|
|
V_[1] -= mat[1];
|
|
V_[2] -= mat[2];
|
|
return *this;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> &operator*= (const T scalar)
|
|
{
|
|
V_[0] *= scalar;
|
|
V_[1] *= scalar;
|
|
V_[2] *= scalar;
|
|
return *this;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> &operator/= (const T scalar)
|
|
{
|
|
if (scalar != (T)0)
|
|
{
|
|
V_[0] /= scalar;
|
|
V_[1] /= scalar;
|
|
V_[2] /= scalar;
|
|
}
|
|
return *this;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
ColumnMatrix3<T> Transpose() const
|
|
{
|
|
return ColumnMatrix3<T>(V_[0][0], V_[0][1], V_[0][2], V_[1][0], V_[1][1], V_[1][2], V_[2][0], V_[2][1], V_[2][2], false);
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////
|
|
};
|
|
|
|
template <typename T>
|
|
std::ostream& operator<<(std::ostream& s, const ColumnMatrix3<T>& A)
|
|
{
|
|
s << A[0][0] << ' ' << A[1][0] << ' ' << A[2][0] << std::endl;
|
|
s << A[0][1] << ' ' << A[1][1] << ' ' << A[2][1] << std::endl;
|
|
s << A[0][2] << ' ' << A[1][2] << ' ' << A[2][2] << std::endl;
|
|
return s;
|
|
}
|
|
|
|
|
|
typedef ColumnMatrix3<double> ColumnMatrix3d;
|
|
typedef ColumnMatrix3<float> ColumnMatrix3f;
|
|
|
|
#endif
|
|
|