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

#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