kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Merge pull request #22015 from 3x380V/matrix4d_fixes

Browse Source 
Matrix4D fixes
This commit is contained in:
Kacper Donat
2025-06-20 22:39:44 +02:00
committed by GitHub
parent a6485d1ae1 11a05b71b7
commit 273a84c822
3 changed files with 264 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
src/Base/Matrix.cpp
View File

@@ -35,30 +35,30 @@ using namespace Base;
// clang-format off
Matrix4D::Matrix4D()
: dMtrx4D {{1., 0., 0., 0.},
{0., 1., 0., 0.},
{0., 0., 1., 0.},
{0., 0., 0., 1.}}
: dMtrx4D {{{1., 0., 0., 0.},
{0., 1., 0., 0.},
{0., 0., 1., 0.},
{0., 0., 0., 1.}}}
{}
Matrix4D::Matrix4D(float a11, float a12, float a13, float a14,
float a21, float a22, float a23, float a24,
float a31, float a32, float a33, float a34,
float a41, float a42, float a43, float a44)
: dMtrx4D {{a11, a12, a13, a14},
{a21, a22, a23, a24},
{a31, a32, a33, a34},
{a41, a42, a43, a44}}
: dMtrx4D {{{a11, a12, a13, a14},
{a21, a22, a23, a24},
{a31, a32, a33, a34},
{a41, a42, a43, a44}}}
{}
Matrix4D::Matrix4D(double a11, double a12, double a13, double a14,
double a21, double a22, double a23, double a24,
double a31, double a32, double a33, double a34,
double a41, double a42, double a43, double a44)
: dMtrx4D {{a11, a12, a13, a14},
{a21, a22, a23, a24},
{a31, a32, a33, a34},
{a41, a42, a43, a44}}
: dMtrx4D {{{a11, a12, a13, a14},
{a21, a22, a23, a24},
{a31, a32, a33, a34},
{a41, a42, a43, a44}}}
{}
// clang-format on
@@ -277,13 +277,9 @@ void Matrix4D::rotLine(const Vector3d& vec, double fAngle)
double fsin {};
// set all entries to "0"
for (short iz = 0; iz < 4; iz++) {
for (short is = 0; is < 4; is++) {
clMA.dMtrx4D[iz][is] = 0;
clMB.dMtrx4D[iz][is] = 0;
clMC.dMtrx4D[iz][is] = 0;
}
}
clMA.nullify();
clMB.nullify();
clMC.nullify();
// ** normalize the rotation axis
clRotAxis.Normalize();
@@ -313,10 +309,9 @@ void Matrix4D::rotLine(const Vector3d& vec, double fAngle)
clMC.dMtrx4D[2][0] = -fsin * clRotAxis.y;
clMC.dMtrx4D[2][1] = fsin * clRotAxis.x;
for (short iz = 0; iz < 3; iz++) {
for (short is = 0; is < 3; is++) {
clMRot.dMtrx4D[iz][is] =
clMA.dMtrx4D[iz][is] + clMB.dMtrx4D[iz][is] + clMC.dMtrx4D[iz][is];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
clMRot.dMtrx4D[i][j] = clMA.dMtrx4D[i][j] + clMB.dMtrx4D[i][j] + clMC.dMtrx4D[i][j];
}
}
@@ -522,15 +517,15 @@ void Matrix4D::inverse()
/**** Herausnehmen und Inversion der TranslationsMatrix
aus der TransformationMatrix ****/
for (short iz = 0; iz < 3; iz++) {
clInvTrlMat.dMtrx4D[iz][3] = -dMtrx4D[iz][3];
for (int i = 0; i < 3; i++) {
clInvTrlMat.dMtrx4D[i][3] = -dMtrx4D[i][3];
}
/**** Herausnehmen und Inversion der RotationsMatrix
aus der TransformationMatrix ****/
for (short iz = 0; iz < 3; iz++) {
for (short is = 0; is < 3; is++) {
clInvRotMat.dMtrx4D[iz][is] = dMtrx4D[is][iz];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
clInvRotMat.dMtrx4D[i][j] = dMtrx4D[j][i];
}
}
@@ -624,7 +619,7 @@ void Matrix4D::inverseOrthogonal()
{
Base::Vector3d vec(dMtrx4D[0][3], dMtrx4D[1][3], dMtrx4D[2][3]);
transpose();
vec = this->operator*(vec);
multVec(vec, vec);
dMtrx4D[0][3] = -vec.x;
dMtrx4D[3][0] = 0;
dMtrx4D[1][3] = -vec.y;
@@ -651,36 +646,36 @@ void Matrix4D::inverseGauss()
void Matrix4D::getMatrix(double dMtrx[16]) const
{
for (short iz = 0; iz < 4; iz++) {
for (short is = 0; is < 4; is++) {
dMtrx[4 * iz + is] = dMtrx4D[iz][is];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
dMtrx[4 * i + j] = dMtrx4D[i][j];
}
}
}
void Matrix4D::setMatrix(const double dMtrx[16])
{
for (short iz = 0; iz < 4; iz++) {
for (short is = 0; is < 4; is++) {
dMtrx4D[iz][is] = dMtrx[4 * iz + is];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
dMtrx4D[i][j] = dMtrx[4 * i + j];
}
}
}
void Matrix4D::getGLMatrix(double dMtrx[16]) const
{
for (short iz = 0; iz < 4; iz++) {
for (short is = 0; is < 4; is++) {
dMtrx[iz + 4 * is] = dMtrx4D[iz][is];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
dMtrx[i + 4 * j] = dMtrx4D[i][j];
}
}
}
void Matrix4D::setGLMatrix(const double dMtrx[16])
{
for (short iz = 0; iz < 4; iz++) {
for (short is = 0; is < 4; is++) {
dMtrx4D[iz][is] = dMtrx[iz + 4 * is];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
dMtrx4D[i][j] = dMtrx[i + 4 * j];
}
}
}
@@ -693,7 +688,7 @@ unsigned long Matrix4D::getMemSpace()
void Matrix4D::Print() const
{
// NOLINTBEGIN
for (short i = 0; i < 4; i++) {
for (int i = 0; i < 4; i++) {
printf("%9.3f %9.3f %9.3f %9.3f\n",
dMtrx4D[i][0],
dMtrx4D[i][1],
@@ -705,15 +700,12 @@ void Matrix4D::Print() const
void Matrix4D::transpose()
{
double dNew[4][4];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
dNew[j][i] = dMtrx4D[i][j];
}
}
memcpy(dMtrx4D, dNew, sizeof(dMtrx4D));
std::swap(dMtrx4D[0][1], dMtrx4D[1][0]);
std::swap(dMtrx4D[0][2], dMtrx4D[2][0]);
std::swap(dMtrx4D[0][3], dMtrx4D[3][0]);
std::swap(dMtrx4D[1][2], dMtrx4D[2][1]);
std::swap(dMtrx4D[1][3], dMtrx4D[3][1]);
std::swap(dMtrx4D[2][3], dMtrx4D[3][2]);
}
@@ -788,8 +780,8 @@ std::string Matrix4D::analyse() const
trp.transpose();
trp = trp * sub;
bool ortho = true;
for (unsigned short i = 0; i < 4 && ortho; i++) {
for (unsigned short j = 0; j < 4 && ortho; j++) {
for (unsigned int i = 0; i < 4 && ortho; i++) {
for (unsigned int j = 0; j < 4 && ortho; j++) {
if (i != j) {
if (fabs(trp[i][j]) > eps) {
ortho = false;

132
src/Base/Matrix.h
View File

@@ -106,13 +106,13 @@ public:
/// Comparison
inline bool operator==(const Matrix4D& mat) const;
/// Index operator
inline double* operator[](unsigned short usNdx);
inline std::array<double, 4>& operator[](unsigned int usNdx);
/// Index operator
inline const double* operator[](unsigned short usNdx) const;
inline const std::array<double, 4>& operator[](unsigned int usNdx) const;
/// Get vector of row
inline Vector3d getRow(unsigned short usNdx) const;
inline Vector3d getRow(unsigned int usNdx) const;
/// Get vector of column
inline Vector3d getCol(unsigned short usNdx) const;
inline Vector3d getCol(unsigned int usNdx) const;
/// Get vector of diagonal
inline Vector3d diagonal() const;
/// Get trace of the 3x3 matrix
@@ -120,9 +120,9 @@ public:
/// Get trace of the 4x4 matrix
inline double trace() const;
/// Set row to vector
inline void setRow(unsigned short usNdx, const Vector3d& vec);
inline void setRow(unsigned int usNdx, const Vector3d& vec);
/// Set column to vector
inline void setCol(unsigned short usNdx, const Vector3d& vec);
inline void setCol(unsigned int usNdx, const Vector3d& vec);
/// Set diagonal to vector
inline void setDiagonal(const Vector3d& vec);
/// Compute the determinant of the matrix
@@ -234,27 +234,21 @@ public:
void fromString(const std::string& str);
private:
double dMtrx4D[4][4];
using Array2d = std::array<std::array<double, 4>, 4>;
Array2d dMtrx4D;
};
inline Matrix4D Matrix4D::operator+(const Matrix4D& mat) const
{
Matrix4D clMat;
for (int iz = 0; iz < 4; iz++) {
for (int is = 0; is < 4; is++) {
clMat.dMtrx4D[iz][is] = dMtrx4D[iz][is] + mat[iz][is];
}
}
return clMat;
Matrix4D newMat(*this);
return newMat += mat;
}
inline Matrix4D& Matrix4D::operator+=(const Matrix4D& mat)
{
for (int iz = 0; iz < 4; iz++) {
for (int is = 0; is < 4; is++) {
dMtrx4D[iz][is] += mat[iz][is];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
dMtrx4D[i][j] += mat[i][j];
}
}
@@ -263,22 +257,15 @@ inline Matrix4D& Matrix4D::operator+=(const Matrix4D& mat)
inline Matrix4D Matrix4D::operator-(const Matrix4D& mat) const
{
Matrix4D clMat;
for (int iz = 0; iz < 4; iz++) {
for (int is = 0; is < 4; is++) {
clMat.dMtrx4D[iz][is] = dMtrx4D[iz][is] - mat[iz][is];
}
}
return clMat;
Matrix4D newMat(*this);
return newMat -= mat;
}
inline Matrix4D& Matrix4D::operator-=(const Matrix4D& mat)
{
for (int iz = 0; iz < 4; iz++) {
for (int is = 0; is < 4; is++) {
dMtrx4D[iz][is] -= mat[iz][is];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
dMtrx4D[i][j] -= mat[i][j];
}
}
@@ -287,19 +274,7 @@ inline Matrix4D& Matrix4D::operator-=(const Matrix4D& mat)
inline Matrix4D& Matrix4D::operator*=(const Matrix4D& mat)
{
Matrix4D clMat;
for (int iz = 0; iz < 4; iz++) {
for (int is = 0; is < 4; is++) {
clMat.dMtrx4D[iz][is] = 0;
for (int ie = 0; ie < 4; ie++) {
clMat.dMtrx4D[iz][is] += dMtrx4D[iz][ie] * mat.dMtrx4D[ie][is];
}
}
}
(*this) = clMat;
(*this) = (*this) * mat;
return *this;
}
@@ -307,11 +282,11 @@ inline Matrix4D Matrix4D::operator*(const Matrix4D& mat) const
{
Matrix4D clMat;
for (int iz = 0; iz < 4; iz++) {
for (int is = 0; is < 4; is++) {
clMat.dMtrx4D[iz][is] = 0;
for (int ie = 0; ie < 4; ie++) {
clMat.dMtrx4D[iz][is] += dMtrx4D[iz][ie] * mat.dMtrx4D[ie][is];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
clMat.dMtrx4D[i][j] = 0;
for (int e = 0; e < 4; e++) {
clMat.dMtrx4D[i][j] += dMtrx4D[i][e] * mat.dMtrx4D[e][j];
}
}
}
@@ -325,9 +300,9 @@ inline Matrix4D& Matrix4D::operator=(const Matrix4D& mat)
return *this;
}
for (int iz = 0; iz < 4; iz++) {
for (int is = 0; is < 4; is++) {
dMtrx4D[iz][is] = mat.dMtrx4D[iz][is];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
dMtrx4D[i][j] = mat.dMtrx4D[i][j];
}
}
@@ -336,23 +311,16 @@ inline Matrix4D& Matrix4D::operator=(const Matrix4D& mat)
inline Vector3f Matrix4D::operator*(const Vector3f& vec) const
{
// clang-format off
double sx = static_cast<double>(vec.x);
double sy = static_cast<double>(vec.y);
double sz = static_cast<double>(vec.z);
return Vector3f(static_cast<float>(dMtrx4D[0][0] * sx + dMtrx4D[0][1] * sy + dMtrx4D[0][2] * sz + dMtrx4D[0][3]),
static_cast<float>(dMtrx4D[1][0] * sx + dMtrx4D[1][1] * sy + dMtrx4D[1][2] * sz + dMtrx4D[1][3]),
static_cast<float>(dMtrx4D[2][0] * sx + dMtrx4D[2][1] * sy + dMtrx4D[2][2] * sz + dMtrx4D[2][3]));
// clang-format on
Vector3f dst;
multVec(vec, dst);
return dst;
}
inline Vector3d Matrix4D::operator*(const Vector3d& vec) const
{
// clang-format off
return Vector3d((dMtrx4D[0][0] * vec.x + dMtrx4D[0][1] * vec.y + dMtrx4D[0][2] * vec.z + dMtrx4D[0][3]),
(dMtrx4D[1][0] * vec.x + dMtrx4D[1][1] * vec.y + dMtrx4D[1][2] * vec.z + dMtrx4D[1][3]),
(dMtrx4D[2][0] * vec.x + dMtrx4D[2][1] * vec.y + dMtrx4D[2][2] * vec.z + dMtrx4D[2][3]));
// clang-format on
Vector3d dst;
multVec(vec, dst);
return dst;
}
inline void Matrix4D::multVec(const Vector3d& src, Vector3d& dst) const
@@ -379,21 +347,15 @@ inline void Matrix4D::multVec(const Vector3f& src, Vector3f& dst) const
inline Matrix4D Matrix4D::operator*(double scalar) const
{
Matrix4D matrix;
for (unsigned short i = 0; i < 4; i++) {
for (unsigned short j = 0; j < 4; j++) {
matrix.dMtrx4D[i][j] = dMtrx4D[i][j] * scalar;
}
}
return matrix;
Matrix4D newMat(*this);
return newMat *= scalar;
}
inline Matrix4D& Matrix4D::operator*=(double scalar)
{
// NOLINTBEGIN
for (unsigned short i = 0; i < 4; i++) {
for (unsigned short j = 0; j < 4; j++) {
for (unsigned int i = 0; i < 4; i++) {
for (unsigned int j = 0; j < 4; j++) {
dMtrx4D[i][j] *= scalar;
}
}
@@ -403,9 +365,9 @@ inline Matrix4D& Matrix4D::operator*=(double scalar)
inline bool Matrix4D::operator==(const Matrix4D& mat) const
{
for (int iz = 0; iz < 4; iz++) {
for (int is = 0; is < 4; is++) {
if (fabs(dMtrx4D[iz][is] - mat.dMtrx4D[iz][is]) > traits_type::epsilon()) {
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
if (fabs(dMtrx4D[i][j] - mat.dMtrx4D[i][j]) > traits_type::epsilon()) {
return false;
}
}
@@ -421,26 +383,26 @@ inline bool Matrix4D::operator!=(const Matrix4D& mat) const
inline Vector3f& operator*=(Vector3f& vec, const Matrix4D& mat)
{
vec = mat * vec;
mat.multVec(vec, vec);
return vec;
}
inline double* Matrix4D::operator[](unsigned short usNdx)
inline std::array<double, 4>& Matrix4D::operator[](unsigned int usNdx)
{
return dMtrx4D[usNdx];
}
inline const double* Matrix4D::operator[](unsigned short usNdx) const
inline const std::array<double, 4>& Matrix4D::operator[](unsigned int usNdx) const
{
return dMtrx4D[usNdx];
}
inline Vector3d Matrix4D::getRow(unsigned short usNdx) const
inline Vector3d Matrix4D::getRow(unsigned int usNdx) const
{
return Vector3d(dMtrx4D[usNdx][0], dMtrx4D[usNdx][1], dMtrx4D[usNdx][2]);
}
inline Vector3d Matrix4D::getCol(unsigned short usNdx) const
inline Vector3d Matrix4D::getCol(unsigned int usNdx) const
{
return Vector3d(dMtrx4D[0][usNdx], dMtrx4D[1][usNdx], dMtrx4D[2][usNdx]);
}
@@ -460,14 +422,14 @@ inline double Matrix4D::trace() const
return dMtrx4D[0][0] + dMtrx4D[1][1] + dMtrx4D[2][2] + dMtrx4D[3][3];
}
inline void Matrix4D::setRow(unsigned short usNdx, const Vector3d& vec)
inline void Matrix4D::setRow(unsigned int usNdx, const Vector3d& vec)
{
dMtrx4D[usNdx][0] = vec.x;
dMtrx4D[usNdx][1] = vec.y;
dMtrx4D[usNdx][2] = vec.z;
}
inline void Matrix4D::setCol(unsigned short usNdx, const Vector3d& vec)
inline void Matrix4D::setCol(unsigned int usNdx, const Vector3d& vec)
{
dMtrx4D[0][usNdx] = vec.x;
dMtrx4D[1][usNdx] = vec.y;