josh/j3ml
1
0
Fork 1
Code Issues 12 Pull Requests Actions Packages Projects Releases 41 Wiki Activity
Files
36e1f398a77c4f1e4f28a36a552b1d95f03947f0
j3ml/src/J3ML/LinearAlgebra/Matrix4x4.cpp

1053 lines
39 KiB
C++
Raw Blame History

#include <J3ML/LinearAlgebra/Matrix4x4.h>
#include <J3ML/LinearAlgebra/Vector4.h>
namespace J3ML::LinearAlgebra {
const Matrix4x4 Matrix4x4::Zero = Matrix4x4(0.f);
const Matrix4x4 Matrix4x4::Identity = Matrix4x4({1,0,0,0}, {0,1,0,0}, {0,0,1,0}, {0,0,0,1});
const Matrix4x4 Matrix4x4::NaN = Matrix4x4(NAN);
Matrix4x4::Matrix4x4(const Vector4 &r1, const Vector4 &r2, const Vector4 &r3, const Vector4 &r4) {
this->elems[0][0] = r1.x;
this->elems[0][1] = r1.y;
this->elems[0][2] = r1.z;
this->elems[0][3] = r1.w;
this->elems[1][0] = r2.x;
this->elems[1][1] = r2.y;
this->elems[1][2] = r2.z;
this->elems[1][3] = r2.w;
this->elems[2][0] = r3.x;
this->elems[2][1] = r3.y;
this->elems[2][2] = r3.z;
this->elems[2][3] = r3.w;
this->elems[3][0] = r4.x;
this->elems[3][1] = r4.y;
this->elems[3][2] = r4.z;
this->elems[3][3] = r4.w;
}
Matrix4x4::Matrix4x4(float val) {
this->elems[0][0] = val;
this->elems[0][1] = val;
this->elems[0][2] = val;
this->elems[0][3] = val;
this->elems[1][0] = val;
this->elems[1][1] = val;
this->elems[1][2] = val;
this->elems[1][3] = val;
this->elems[2][0] = val;
this->elems[2][1] = val;
this->elems[2][2] = val;
this->elems[2][3] = val;
this->elems[3][0] = val;
this->elems[3][1] = val;
this->elems[3][2] = val;
this->elems[3][3] = val;
}
Matrix4x4::Matrix4x4(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13,
float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) {
this->elems[0][0] = m00;
this->elems[0][1] = m01;
this->elems[0][2] = m02;
this->elems[0][3] = m03;
this->elems[1][0] = m10;
this->elems[1][1] = m11;
this->elems[1][2] = m12;
this->elems[1][3] = m13;
this->elems[2][0] = m20;
this->elems[2][1] = m21;
this->elems[2][2] = m22;
this->elems[2][3] = m23;
this->elems[3][0] = m30;
this->elems[3][1] = m31;
this->elems[3][2] = m32;
this->elems[3][3] = m33;
}
Matrix4x4::Matrix4x4(const Quaternion &orientation) {
}
void Matrix4x4::SetTranslatePart(float translateX, float translateY, float translateZ) {
elems[0][3] = translateX;
elems[1][3] = translateY;
elems[2][3] = translateZ;
}
void Matrix4x4::SetTranslatePart(const Vector3 &offset) {
elems[0][3] = offset.x;
elems[1][3] = offset.y;
elems[2][3] = offset.z;
}
void Matrix4x4::SetRotatePart(const Quaternion& q)
{
// See e.g. http://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .
const float x = q.x;
const float y = q.y;
const float z = q.z;
const float w = q.w;
elems[0][0] = 1 - 2*(y*y + z*z); elems[0][1] = 2*(x*y - z*w); elems[0][2] = 2*(x*z + y*w);
elems[1][0] = 2*(x*y + z*w); elems[1][1] = 1 - 2*(x*x + z*z); elems[1][2] = 2*(y*z - x*w);
elems[2][0] = 2*(x*z - y*w); elems[2][1] = 2*(y*z + x*w); elems[2][2] = 1 - 2*(x*x + y*y);
}
void Matrix4x4::Set3x3Part(const Matrix3x3& r)
{
At(0, 0) = r[0][0]; At(0, 1) = r[0][1]; At(0, 2) = r[0][2];
At(1, 0) = r[1][0]; At(1, 1) = r[1][1]; At(1, 2) = r[1][2];
At(2, 0) = r[2][0]; At(2, 1) = r[2][1]; At(2, 2) = r[2][2];
}
void Matrix4x4::SetRow(int row, const Vector3 &rowVector, float m_r3) {
SetRow(row, rowVector.x, rowVector.y, rowVector.z, m_r3);
}
void Matrix4x4::SetRow(int row, const Vector4 &rowVector) {
SetRow(row, rowVector.x, rowVector.y, rowVector.z, rowVector.w);
}
void Matrix4x4::SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3) {
elems[row][0] = m_r0;
elems[row][1] = m_r1;
elems[row][2] = m_r2;
elems[row][3] = m_r3;
}
Matrix4x4::Matrix4x4(const Quaternion &orientation, const Vector3 &translation) {
SetRotatePart(orientation);
SetTranslatePart(translation);
SetRow(3, 0, 0, 0, 1);
}
Matrix4x4 Matrix4x4::D3DOrthoProjLH(float n, float f, float h, float v) {
float p00 = 2.f / h; float p01 = 0; float p02 = 0; float p03 = 0.f;
float p10 = 0; float p11 = 2.f / v; float p12 = 0; float p13 = 0.f;
float p20 = 0; float p21 = 0; float p22 = 1.f / (f-n); float p23 = n / (n-f);
float p30 = 0; float p31 = 0; float p32 = 0.f; float p33 = 1.f;
return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
}
/** This function generates an orthographic projection matrix that maps from
the Direct3D view space to the Direct3D normalized viewport space as follows:
In Direct3D view space, we assume that the camera is positioned at the origin (0,0,0).
The camera looks directly towards the positive Z axis (0,0,1).
The -X axis spans to the left of the screen, +X goes to the right.
-Y goes to the bottom of the screen, +Y goes to the top.
After the transformation, we're in the Direct3D normalized viewport space as follows:
(-1,-1,0) is the bottom-left corner of the viewport at the near plane.
(1,1,0) is the top-right corner of the viewport at the near plane.
(0,0,0) is the center point at the near plane.
Coordinates with z=1 are at the far plane.
Examples:
(0,0,n) maps to (0,0,0).
(0,0,f) maps to (0,0,1).
(-h/2, -v/2, n) maps to (-1, -1, 0).
(h/2, v/2, f) maps to (1, 1, 1).
*/
Matrix4x4 Matrix4x4::D3DOrthoProjRH(float n, float f, float h, float v)
{
// D3DOrthoProjLH and D3DOrthoProjRH differ from each other in that the third column is negated.
// This corresponds to LH = RH * In, where In is a diagonal matrix with elements [1 1 -1 1].
float p00 = 2.f / h; float p01 = 0; float p02 = 0; float p03 = 0.f;
float p10 = 0; float p11 = 2.f / v; float p12 = 0; float p13 = 0.f;
float p20 = 0; float p21 = 0; float p22 = 1.f / (n-f); float p23 = n / (n-f);
float p30 = 0; float p31 = 0; float p32 = 0.f; float p33 = 1.f;
return {p00,p01,p02,p03, p10, p11, p12, p13, p20,p21,p22,p23, p30,p31,p32,p33};
}
float Matrix4x4::At(int x, int y) const {
return elems[x][y];
}
Matrix4x4 Matrix4x4::operator*(const Matrix4x4 &rhs) const {
float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0) + At(0, 3) * rhs.At(3, 0);
float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1) + At(0, 3) * rhs.At(3, 1);
float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2) + At(0, 3) * rhs.At(3, 2);
float r03 = At(0, 0) * rhs.At(0, 3) + At(0, 1) * rhs.At(1, 3) + At(0, 2) * rhs.At(2, 3) + At(0, 3) * rhs.At(3, 3);
float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0) + At(1, 3) * rhs.At(3, 0);
float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1) + At(1, 3) * rhs.At(3, 1);
float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2) + At(1, 3) * rhs.At(3, 2);
float r13 = At(1, 0) * rhs.At(0, 3) + At(1, 1) * rhs.At(1, 3) + At(1, 2) * rhs.At(2, 3) + At(1, 3) * rhs.At(3, 3);
float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0) + At(2, 3) * rhs.At(3, 0);
float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1) + At(2, 3) * rhs.At(3, 1);
float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2) + At(2, 3) * rhs.At(3, 2);
float r23 = At(2, 0) * rhs.At(0, 3) + At(2, 1) * rhs.At(1, 3) + At(2, 2) * rhs.At(2, 3) + At(2, 3) * rhs.At(3, 3);
float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0) + At(3, 3) * rhs.At(3, 0);
float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1) + At(3, 3) * rhs.At(3, 1);
float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2) + At(3, 3) * rhs.At(3, 2);
float r33 = At(3, 0) * rhs.At(0, 3) + At(3, 1) * rhs.At(1, 3) + At(3, 2) * rhs.At(2, 3) + At(3, 3) * rhs.At(3, 3);
return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
}
Vector4 Matrix4x4::operator[](int row) {
return Vector4{elems[row][0], elems[row][1], elems[row][2], elems[row][3]};
}
Matrix4x4 Matrix4x4::operator*(const Matrix3x3 &rhs) const {
float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);
float r03 = At(0, 3);
float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2);
float r13 = At(1, 3);
float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0);
float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1);
float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2);
float r23 = At(2, 3);
float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0);
float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1);
float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2);
float r33 = At(3, 3);
return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
}
Matrix4x4 Matrix4x4::operator+() const { return *this; }
Matrix4x4 Matrix4x4::FromTranslation(const Vector3 &rhs) {
return Matrix4x4(1.f, 0, 0, rhs.x,
0, 1.f, 0, rhs.y,
0, 0, 1.f, rhs.z,
0, 0, 0, 1.f);
}
Vector3 Matrix4x4::Transform(const Vector3 &rhs) const {
return Transform(rhs.x, rhs.y, rhs.z);
}
Vector3 Matrix4x4::Transform(float tx, float ty, float tz) const {
return Vector3(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) * tz + At(0, 3),
At(1, 0) * tx + At(1, 1) * ty + At(1, 2) * tz + At(1, 3),
At(2, 0) * tx + At(2, 1) * ty + At(2, 2) * tz + At(2, 3));
}
Vector2 Matrix4x4::Transform(float tx, float ty) const {
return Vector2(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) + At(0, 3),
At(1, 0) * tx + At(1, 1) * ty + At(1, 2) + At(1, 3));
}
Vector2 Matrix4x4::Transform(const Vector2 &rhs) const {
return Transform(rhs.x, rhs.y);
}
Matrix4x4 &Matrix4x4::operator=(const Matrix3x3 &rhs) {
Set3x3Part(rhs);
SetTranslatePart(0,0,0);
SetRow(3, 0, 0, 0, 1);
return *this;
}
Matrix4x4 &Matrix4x4::operator=(const Quaternion &rhs) {
*this = rhs.ToMatrix4x4();
return *this;
}
float &Matrix4x4::At(int row, int col) {
return elems[row][col];
}
Matrix4x4 Matrix4x4::Inverse() const {
// Compute the inverse directly using Cramer's rule
// Warning: This method is numerically very unstable!
float d = Determinant();
d = 1.f / d;
float a11 = At(0, 0);float a12 = At(0, 1);float a13 = At(0, 2);float a14 = At(0, 3);
float a21 = At(1, 0);float a22 = At(1, 1);float a23 = At(1, 2);float a24 = At(1, 3);
float a31 = At(2, 0);float a32 = At(2, 1);float a33 = At(2, 2);float a34 = At(2, 3);
float a41 = At(3, 0);float a42 = At(3, 1);float a43 = At(3, 2);float a44 = At(3, 3);
Matrix4x4 i = {
d * (a22*a33*a44 + a23*a34*a42 + a24*a32*a43 - a22*a34*a43 - a23*a32*a44 - a24*a33*a42),
d * (a12*a34*a43 + a13*a32*a44 + a14*a33*a42 - a12*a33*a44 - a13*a34*a42 - a14*a32*a43),
d * (a12*a23*a44 + a13*a24*a42 + a14*a22*a43 - a12*a24*a43 - a13*a22*a44 - a14*a23*a42),
d * (a12*a24*a33 + a13*a22*a34 + a14*a23*a32 - a12*a23*a34 - a13*a24*a32 - a14*a22*a33),
d * (a21*a34*a43 + a23*a31*a44 + a24*a33*a41 - a21*a33*a44 - a23*a34*a41 - a24*a31*a43),
d * (a11*a33*a44 + a13*a34*a41 + a14*a31*a43 - a11*a34*a43 - a13*a31*a44 - a14*a33*a41),
d * (a11*a24*a43 + a13*a21*a44 + a14*a23*a41 - a11*a23*a44 - a13*a24*a41 - a14*a21*a43),
d * (a11*a23*a34 + a13*a24*a31 + a14*a21*a33 - a11*a24*a33 - a13*a21*a34 - a14*a23*a31),
d * (a21*a32*a44 + a22*a34*a41 + a24*a31*a42 - a21*a34*a42 - a22*a31*a44 - a24*a32*a41),
d * (a11*a34*a42 + a12*a31*a44 + a14*a32*a41 - a11*a32*a44 - a12*a34*a41 - a14*a31*a42),
d * (a11*a22*a44 + a12*a24*a41 + a14*a21*a42 - a11*a24*a42 - a12*a21*a44 - a14*a22*a41),
d * (a11*a24*a32 + a12*a21*a34 + a14*a22*a31 - a11*a22*a34 - a12*a24*a31 - a14*a21*a32),
d * (a21*a33*a42 + a22*a31*a43 + a23*a32*a41 - a21*a32*a43 - a22*a33*a41 - a23*a31*a42),
d * (a11*a32*a43 + a12*a33*a41 + a13*a31*a42 - a11*a33*a42 - a12*a31*a43 - a13*a32*a41),
d * (a11*a23*a42 + a12*a21*a43 + a13*a22*a41 - a11*a22*a43 - a12*a23*a41 - a13*a21*a42),
d * (a11*a22*a33 + a12*a23*a31 + a13*a21*a32 - a11*a23*a32 - a12*a21*a33 - a13*a22*a31)
};
return i;
}
float Matrix4x4::Minor(int i, int j) const {
int r0 = SKIPNUM(0, i);
int r1 = SKIPNUM(1, i);
int r2 = SKIPNUM(2, i);
int c0 = SKIPNUM(0, j);
int c1 = SKIPNUM(1, j);
int c2 = SKIPNUM(2, j);
float a = At(r0, c0);
float b = At(r0, c1);
float c = At(r0, c2);
float d = At(r1, c0);
float e = At(r1, c1);
float f = At(r1, c2);
float g = At(r2, c0);
float h = At(r2, c1);
float k = At(r2, c2);
return a*e*k + b*f*g + c*d*h - a*f*h - b*d*k - c*e*g;
}
float Matrix4x4::Determinant() const {
return At(0, 0) * Minor(0,0) - At(0, 1) * Minor(0,1) + At(0, 2) * Minor(0,2) - At(0, 3) * Minor(0,3);
}
float Matrix4x4::Determinant3x3() const {
const float a = elems[0][0];
const float b = elems[0][1];
const float c = elems[0][2];
const float d = elems[1][0];
const float e = elems[1][1];
const float f = elems[1][2];
const float g = elems[2][0];
const float h = elems[2][1];
const float i = elems[2][2];
return a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g;
}
Matrix3x3 Matrix4x4::GetRotatePart() const {
return Matrix3x3 {
At(0, 0), At(0, 1), At(0, 2),
At(1, 0), At(1, 1), At(1, 2),
At(2, 0), At(2, 1), At(2, 2)
};
}
Matrix4x4 Matrix4x4::Transpose() const {
Matrix4x4 copy;
copy.elems[0][0] = elems[0][0]; copy.elems[0][1] = elems[1][0]; copy.elems[0][2] = elems[2][0]; copy.elems[0][3] = elems[3][0];
copy.elems[1][0] = elems[0][1]; copy.elems[1][1] = elems[1][1]; copy.elems[1][2] = elems[2][1]; copy.elems[1][3] = elems[3][1];
copy.elems[2][0] = elems[0][2]; copy.elems[2][1] = elems[1][2]; copy.elems[2][2] = elems[2][2]; copy.elems[2][3] = elems[3][2];
copy.elems[3][0] = elems[0][3]; copy.elems[3][1] = elems[1][3]; copy.elems[3][2] = elems[2][3]; copy.elems[3][3] = elems[3][3];
return copy;
}
Vector4 Matrix4x4::Diagonal() const {
return Vector4{At(0, 0), At(1,1), At(2,2), At(3,3)};
}
Vector3 Matrix4x4::Diagonal3() const {
return Vector3 { At(0, 0), At(1,1), At(2,2) };
}
Vector3 Matrix4x4::WorldX() const {
return GetColumn3(0);
}
Vector3 Matrix4x4::WorldY() const {
return GetColumn3(1);
}
Vector3 Matrix4x4::WorldZ() const {
return GetColumn3(2);
}
bool Matrix4x4::IsFinite() const {
for(int iy = 0; iy < Rows; ++iy)
for(int ix = 0; ix < Cols; ++ix)
if (!std::isfinite(elems[iy][ix]))
return false;
return true;
}
Vector3 Matrix4x4::GetColumn3(int index) const {
return Vector3{At(0, index), At(1, index), At(2, index)};
}
Vector2 Matrix4x4::operator*(const Vector2 &rhs) const { return this->Transform(rhs);}
Vector3 Matrix4x4::operator*(const Vector3 &rhs) const { return this->Transform(rhs);}
Vector4 Matrix4x4::operator*(const Vector4 &rhs) const { return this->Transform(rhs);}
Vector4 Matrix4x4::Transform(float tx, float ty, float tz, float tw) const {
return Transform({tx, ty, tz, tw});
}
Vector4 Matrix4x4::Transform(const Vector4 &rhs) const {
return Vector4(At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z + At(0, 3) * rhs.w,
At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z + At(1, 3) * rhs.w,
At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z + At(2, 3) * rhs.w,
At(3, 0) * rhs.x + At(3, 1) * rhs.y + At(3, 2) * rhs.z + At(3, 3) * rhs.w);
}
Vector3 Matrix4x4::GetTranslatePart() const {
return GetColumn3(3);
}
Matrix4x4
Matrix4x4::LookAt(const Vector3 &localFwd, const Vector3 &targetDir, const Vector3 &localUp, const Vector3 &worldUp) {
Matrix4x4 m;
m.Set3x3Part(Matrix3x3::LookAt(localFwd, targetDir, localUp, worldUp));
m.SetTranslatePart(0,0,0);
m.SetRow(3, 0,0,0,1);
return m;
}
Vector4 Matrix4x4::GetRow(int index) const {
return { At(index, 0), At(index, 1), At(index, 2), At(index, 3)};
}
Vector4 Matrix4x4::GetColumn(int index) const {
return { At(0, index), At(1, index), At(2, index), At(3, index)};
}
Vector3 Matrix4x4::GetRow3(int index) const {
return Vector3{ At(index, 0), At(index, 1), At(index, 2)};
}
void Matrix4x4::SwapColumns(int col1, int col2) {
Swap(At(0, col1), At(0, col2));
Swap(At(1, col1), At(1, col2));
Swap(At(2, col1), At(2, col2));
Swap(At(3, col1), At(3, col2));
}
void Matrix4x4::SwapRows(int row1, int row2) {
Swap(At(row1, 0), At(row2, 0));
Swap(At(row1, 1), At(row2, 1));
Swap(At(row1, 2), At(row2, 2));
Swap(At(row1, 3), At(row2, 3));
}
void Matrix4x4::SwapRows3(int row1, int row2) {
Swap(At(row1, 0), At(row2, 0));
Swap(At(row1, 1), At(row2, 1));
Swap(At(row1, 2), At(row2, 2));
}
void Matrix4x4::Pivot() {
/// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
int rowIndex = 0;
for(int col = 0; col < Cols; ++col)
{
int greatest = rowIndex;
// find the rowIndex k with k >= 1 for which Mkj has the largest absolute value.
for(int i = rowIndex; i < Rows; ++i)
if (std::abs(At(i, col)) > std::abs(At(greatest, col)))
greatest = i;
if (std::abs(At(greatest, col)) != 0)
{
if (rowIndex != greatest)
SwapRows(rowIndex, greatest); // the greatest now in rowIndex
ScaleRow(rowIndex, 1.f/At(rowIndex, col));
for(int r = 0; r < Rows; ++r)
if (r != rowIndex)
SetRow(r, GetRow(r) - GetRow(rowIndex) * At(r, col));
++rowIndex;
}
}
}
void Matrix4x4::ScaleColumn3(int col, float scalar) {
At(0, col) *= scalar;
At(1, col) *= scalar;
At(2, col) *= scalar;
}
void Matrix4x4::ScaleColumn(int col, float scalar) {
At(0, col) *= scalar;
At(1, col) *= scalar;
At(2, col) *= scalar;
At(3, col) *= scalar;
}
void Matrix4x4::ScaleRow3(int row, float scalar) {
At(row, 0) *= scalar;
At(row, 1) *= scalar;
At(row, 2) *= scalar;
}
void Matrix4x4::ScaleRow(int row, float scalar) {
At(row, 0) *= scalar;
At(row, 1) *= scalar;
At(row, 2) *= scalar;
At(row, 3) *= scalar;
}
Matrix4x4 Matrix4x4::OpenGLOrthoProjLH(float n, float f, float h, float v) {
/// Same as OpenGLOrthoProjRH, except that the camera looks towards +Z in view space, instead of -Z.
using f32 = float;
f32 p00 = 2.f / h; f32 p01 = 0; f32 p02 = 0; float p03 = 0.f;
f32 p10 = 0; f32 p11 = 2.f / v; f32 p12 = 0; float p13 = 0.f;
f32 p20 = 0; f32 p21 = 0; f32 p22 = 2.f / (f-n); float p23 = (f+n) / (n-f);
f32 p30 = 0; f32 p31 = 0; f32 p32 = 0; float p33 = 1.f;
return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
}
Matrix4x4 Matrix4x4::OpenGLOrthoProjRH(float n, float f, float h, float v) {
using f32 = float;
f32 p00 = 2.f / h; f32 p01 = 0; f32 p02 = 0; f32 p03 = 0.f;
f32 p10 = 0; f32 p11 = 2.f / v; f32 p12 = 0; f32 p13 = 0.f;
f32 p20 = 0; f32 p21 = 0; f32 p22 = 2.f / (n-f); f32 p23 = (f+n) / (n-f);
f32 p30 = 0; f32 p31 = 0; f32 p32 = 0; f32 p33 = 1.f;
return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
}
Matrix4x4 Matrix4x4::OpenGLPerspProjLH(float n, float f, float h, float v) {
// Same as OpenGLPerspProjRH, except that the camera looks towards +Z in view space, instead of -Z.
using f32 = float;
f32 p00 = 2.f *n / h; f32 p01 = 0; f32 p02 = 0; f32 p03 = 0.f;
f32 p10 = 0; f32 p11 = 2.f * n / v; f32 p12 = 0; f32 p13 = 0.f;
f32 p20 = 0; f32 p21 = 0; f32 p22 = (n+f) / (f-n); f32 p23 = 2.f*n*f / (n-f);
f32 p30 = 0; f32 p31 = 0; f32 p32 = 1.f; f32 p33 = 0.f;
return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
}
Matrix4x4 Matrix4x4::OpenGLPerspProjRH(float n, float f, float h, float v) {
// In OpenGL, the post-perspective unit cube ranges in [-1, 1] in all X, Y and Z directions.
// See http://www.songho.ca/opengl/gl_projectionmatrix.html , unlike in Direct3D, where the
// Z coordinate ranges in [0, 1]. This is the only difference between D3DPerspProjRH and OpenGLPerspProjRH.
using f32 = float;
f32 p00 = 2.f *n / h; f32 p01 = 0; f32 p02 = 0; f32 p03 = 0.f;
f32 p10 = 0; f32 p11 = 2.f * n / v; f32 p12 = 0; f32 p13 = 0.f;
f32 p20 = 0; f32 p21 = 0; f32 p22 = (n+f) / (n-f); f32 p23 = 2.f*n*f / (n-f);
f32 p30 = 0; f32 p31 = 0; f32 p32 = -1.f; f32 p33 = 0.f;
return {p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, p30, p31, p32, p33};
}
Matrix4x4::Matrix4x4(const float *data) {
assert(data);
At(0, 0) = data[0];
At(0, 1) = data[1];
At(0, 2) = data[2];
At(0, 3) = data[3];
At(1, 0) = data[4];
At(1, 1) = data[5];
At(1, 2) = data[6];
At(1, 3) = data[7];
At(2, 0) = data[8];
At(2, 1) = data[9];
At(2, 2) = data[10];
At(2, 3) = data[11];
At(3, 0) = data[12];
At(3, 1) = data[13];
At(3, 2) = data[14];
At(3, 3) = data[15];
}
bool Matrix4x4::ContainsProjection(float epsilon) const {
return GetRow(3).Equals(0.f, 0.f, 0.f, 1.f, epsilon) == false;
}
Vector4 Matrix4x4::Mul(const Vector4 &rhs) const {
return *this * rhs;
}
Vector3 Matrix4x4::Mul(const Vector3 &rhs) const {
return *this * rhs;
}
Vector2 Matrix4x4::Mul(const Vector2 &rhs) const {
return *this * rhs;
}
Vector4 Matrix4x4::operator[](int row) const {
return GetRow(row);
}
bool Matrix4x4::HasUniformScale(float epsilon) const {
Vector3 scale = ExtractScale();
return Math::EqualAbs(scale.x, scale.y, epsilon) && Math::EqualAbs(scale.x, scale.z, epsilon);
}
Vector3 Matrix4x4::ExtractScale() const {
return {GetColumn3(0).Length(), GetColumn3(1).Length(), GetColumn3(2).Length()};
}
bool Matrix4x4::IsColOrthogonal(float epsilon) const {
return IsColOrthogonal3(epsilon);
}
bool Matrix4x4::IsRowOrthogonal(float epsilon) const {
return IsRowOrthogonal3(epsilon);
}
bool Matrix4x4::IsColOrthogonal3(float epsilon) const {
return GetColumn(0).IsPerpendicular(GetColumn(1), epsilon)
&& GetColumn(0).IsPerpendicular(GetColumn(2), epsilon)
&& GetColumn(1).IsPerpendicular(GetColumn(2), epsilon);
}
bool Matrix4x4::IsRowOrthogonal3(float epsilon) const {
return GetRow(0).IsPerpendicular(GetRow(1), epsilon)
&& GetRow(0).IsPerpendicular(GetRow(2), epsilon)
&& GetRow(1).IsPerpendicular(GetRow(2), epsilon);
}
Vector4 Matrix4x4::Col(int i) const { return GetColumn(i);}
Vector4 Matrix4x4::Row(int i) const { return GetRow(i);}
Vector3 Matrix4x4::Col3(int i) const { return GetColumn3(i);}
Vector3 Matrix4x4::Row3(int i) const { return GetRow3(i);}
Vector3 Matrix4x4::TransformDir(float tx, float ty, float tz) const
{
assert(!this->ContainsProjection()); // This function does not divide by w or output it, so cannot have projection.
return Vector3(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) * tz,
At(1, 0) * tx + At(1, 1) * ty + At(1, 2) * tz,
At(2, 0) * tx + At(2, 1) * ty + At(2, 2) * tz);
}
void Matrix4x4::InverseOrthonormal()
{
assert(!ContainsProjection());
// a) Transposed the top-left 3x3 part in-place to produce R^t.
Swap(elems[0][1], elems[1][0]);
Swap(elems[0][2], elems[2][0]);
Swap(elems[1][2], elems[2][1]);
// b) Replace the top-right 3x1 part by computing R^t(-T).
SetTranslatePart(TransformDir(-At(0, 3), -At(1, 3), -At(2, 3)));
}
void Matrix4x4::SetCol(int col, const float *data) {
assert(data != nullptr);
SetCol(col, data[0], data[1], data[2], data[3]);
}
void Matrix4x4::SetCol(int column, float m_0c, float m_1c, float m_2c, float m_3c)
{
assert(column >= 0);
assert(column < Cols);
assert(std::isfinite(m_0c));
assert(std::isfinite(m_1c));
assert(std::isfinite(m_2c));
assert(std::isfinite(m_3c));
At(0, column) = m_0c;
At(1, column) = m_1c;
At(2, column) = m_2c;
At(3, column) = m_3c;
}
void Matrix4x4::SetCol(int column, const Vector3 &columnVector, float m_3c)
{
SetCol(column, columnVector.x, columnVector.y, columnVector.z, m_3c);
}
void Matrix4x4::SetCol(int column, const Vector4 &columnVector)
{
SetCol(column, columnVector.x, columnVector.y, columnVector.z, columnVector.w);
}
void Matrix4x4::Transpose() {
Swap(elems[0][1], elems[1][0]);
Swap(elems[0][2], elems[2][0]);
Swap(elems[0][3], elems[3][0]);
Swap(elems[1][2], elems[2][1]);
Swap(elems[1][3], elems[3][1]);
Swap(elems[2][3], elems[3][2]);
}
Matrix4x4 Matrix4x4::Transposed() const {
Matrix4x4 copy;
copy.elems[0][0] = elems[0][0]; copy.elems[0][1] = elems[1][0]; copy.elems[0][2] = elems[2][0]; copy.elems[0][3] = elems[3][0];
copy.elems[1][0] = elems[0][1]; copy.elems[1][1] = elems[1][1]; copy.elems[1][2] = elems[2][1]; copy.elems[1][3] = elems[3][1];
copy.elems[2][0] = elems[0][2]; copy.elems[2][1] = elems[1][2]; copy.elems[2][2] = elems[2][2]; copy.elems[2][3] = elems[3][2];
copy.elems[3][0] = elems[0][3]; copy.elems[3][1] = elems[1][3]; copy.elems[3][2] = elems[2][3]; copy.elems[3][3] = elems[3][3];
return copy;
}
bool Matrix4x4::InverseTranspose() {
bool success = Inverse();
Transpose();
return success;
}
float Matrix4x4::Trace() const {
assert(IsFinite());
return elems[0][0] + elems[1][1] + elems[2][2] + elems[3][3];
}
bool Matrix4x4::InverseOrthogonalUniformScale() {
assert(!ContainsProjection());
assert(IsColOrthogonal(1e-3f));
assert(HasUniformScale());
Swap(At(0, 1), At(1, 0));
Swap(At(0, 2), At(2, 0));
Swap(At(1, 2), At(2, 1));
float scale = Vector3(At(0,0), At(1, 0), At(2, 0)).LengthSquared();
if (scale == 0.f)
return false;
scale = 1.f / scale;
At(0, 0) *= scale; At(0, 1) *= scale; At(0, 2) *= scale;
At(1, 0) *= scale; At(1, 1) *= scale; At(1, 2) *= scale;
At(2, 0) *= scale; At(2, 1) *= scale; At(2, 2) *= scale;
SetTranslatePart(TransformDir(-At(0, 3), -At(1, 3), -At(2, 3)));
return true;
}
Matrix4x4 Matrix4x4::Inverted() const {
Matrix4x4 copy = *this;
copy.Inverse();
return copy;
}
Vector4 &Matrix4x4::Row(int row) {
assert(row >= 0);
assert(row < Rows);
return reinterpret_cast<Vector4 &> (elems[row]);
}
Vector3 &Matrix4x4::Row3(int row) {
assert(row >= 0);
assert(row < Rows);
return reinterpret_cast<Vector3 &> (elems[row]);
}
Vector4 Matrix4x4::Column(int index) const { return GetColumn(index);}
Vector3 Matrix4x4::GetScale() const {
return Vector3(
GetColumn3(0).Length(),
GetColumn3(1).Length(),
GetColumn3(2).Length());
}
Matrix4x4 Matrix4x4::ShearX(float yFactor, float zFactor) {
return Matrix4x4(1.f, yFactor, zFactor, 0.f,
0.f, 1.f, 0.f, 0.f,
0.f, 0.f, 1.f, 0.f,
0.f, 0.f, 0.f, 1.f);
}
Matrix4x4 Matrix4x4::ShearY(float xFactor, float zFactor) /// [similarOverload: ShearX] [hideIndex]
{
return Matrix4x4(1.f, 0.f, 0.f, 0.f,
xFactor, 1.f, zFactor, 0.f,
0.f, 0.f, 1.f, 0.f,
0.f, 0.f, 0.f, 1.f);
}
Matrix4x4 Matrix4x4::ShearZ(float xFactor, float yFactor) /// [similarOverload: ShearX] [hideIndex]
{
return Matrix4x4(1.f, 0.f, 0.f, 0.f,
0.f, 1.f, 0.f, 0.f,
xFactor, yFactor, 1.f, 0.f,
0.f, 0.f, 0.f, 1.f);
}
Matrix4x4 Matrix4x4::D3DPerspProjLH(float n, float f, float h, float v) {
Matrix4x4 p;
p[0][0] = 2.f * n / h; p[0][1] = 0; p[0][2] = 0; p[0][3] = 0.f;
p[1][0] = 0; p[1][1] = 2.f * n / v; p[1][2] = 0; p[1][3] = 0.f;
p[2][0] = 0; p[2][1] = 0; p[2][2] = f / (f-n); p[2][3] = n * f / (n-f);
p[3][0] = 0; p[3][1] = 0; p[3][2] = 1.f; p[3][3] = 0.f;
}
Matrix4x4 Matrix4x4::D3DPerspProjRH(float n, float f, float h, float v) {
Matrix4x4 p;
p[0][0] = 2.f * n / h; p[0][1] = 0; p[0][2] = 0; p[0][3] = 0.f;
p[1][0] = 0; p[1][1] = 2.f * n / v; p[1][2] = 0; p[1][3] = 0.f;
p[2][0] = 0; p[2][1] = 0; p[2][2] = f / (f-n); p[2][3] = n * f / (n-f);
p[3][0] = 0; p[3][1] = 0; p[3][2] = 1.f; p[3][3] = 0.f;
return p;
}
bool Matrix4x4::Inverse(float epsilon) {
return InverseMatrix(*this, epsilon);
}
bool Matrix4x4::LUDecompose(Matrix4x4 &outLower, Matrix4x4 &outUpper) const {
return LUDecomposeMatrix(*this, outLower, outUpper);
}
bool Matrix4x4::CholeskyDecompose(Matrix4x4 &outL) const {
return CholeskyDecomposeMatrix(*this, outL);
}
Matrix4x4 Matrix4x4::InverseTransposed() const {
Matrix4x4 copy = *this;
copy.Transpose();
copy.Inverse();
return copy;
}
void Matrix4x4::Decompose(Vector3 &translate, Matrix3x3 &rotate, Vector3 &scale) const {
assert(this->IsColOrthogonal3());
assert(Row(3).Equals(0,0,0,1));
assert(this->IsColOrthogonal3()); // Duplicate check?
translate = Col3(3);
rotate = GetRotatePart();
scale.x = rotate.Col3(0).Length();
scale.y = rotate.Col3(1).Length();
scale.z = rotate.Col3(2).Length();
assert(!Math::EqualAbs(scale.x, 0));
assert(!Math::EqualAbs(scale.y, 0));
assert(!Math::EqualAbs(scale.z, 0));
rotate.ScaleCol(0, 1.f / scale.x);
rotate.ScaleCol(1, 1.f / scale.y);
rotate.ScaleCol(2, 1.f / scale.z);
// Test that composing back yields the original Matrix4x4
assert(Matrix4x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
}
Matrix4x4 Matrix4x4::Translate(const Vector3 &translation) {
Matrix4x4 m;
m.SetRow(0, 1, 0, 0, translation.x);
m.SetRow(1, 0, 1, 0, translation.y);
m.SetRow(2, 0, 0, 1, translation.y);
m.SetRow(3, 0, 0, 0, 1.f);
return m;
}
Matrix4x4 Matrix4x4::FromTRS(const Vector3 &translate, const Quaternion &rotate, const Vector3 &scale) {
return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
}
Matrix4x4 Matrix4x4::FromTRS(const Vector3 &translate, const Matrix3x3 &rotate, const Vector3 &scale) {
return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
}
Matrix4x4 Matrix4x4::FromTRS(const Vector3 &translate, const Matrix4x4 &rotate, const Vector3 &scale) {
return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
}
void Matrix4x4::SetCol3(int col, float x, float y, float z) {
// TODO: implement assertations
At(0, col) = x;
At(1, col) = y;
At(2, col) = z;
}
Matrix4x4 Matrix4x4::RotateZ(float radians) {
Matrix4x4 r;
r.SetRotatePartZ(radians);
r.SetRow(3, 0, 0, 0, 1);
r.SetCol3(3, 0, 0, 0);
return r;
}
Matrix4x4 Matrix4x4::RotateZ(float radians, const Vector3 &pointOnAxis) {
return Matrix4x4::Translate(pointOnAxis) * RotateY(radians) * Matrix4x4::Translate(-pointOnAxis);
}
Matrix4x4 Matrix4x4::RotateY(float radians) {
Matrix4x4 r;
r.SetRotatePartY(radians);
r.SetRow(3, 0, 0, 0, 1);
r.SetCol3(3, 0, 0, 0);
return r;
}
Matrix4x4 Matrix4x4::RotateY(float radians, const Vector3 &pointOnAxis) {
return Matrix4x4::Translate(pointOnAxis) * RotateY(radians) * Matrix4x4::Translate(-pointOnAxis);
}
Matrix4x4 Matrix4x4::RotateX(float radians) {
Matrix4x4 r;
r.SetRotatePartX(radians);
r.SetRow(3, 0, 0, 0, 1);
r.SetCol3(3, 0, 0, 0);
return r;
}
Matrix4x4 Matrix4x4::RotateX(float radians, const Vector3 &pointOnAxis) {
return Matrix4x4::Translate(pointOnAxis) * RotateX(radians) * Matrix4x4::Translate(-pointOnAxis);
}
Matrix4x4 Matrix4x4::Scale(const Vector3 &scale) {
Matrix4x4 m;
m.SetRow(0, scale.x, 0, 0, 0);
m.SetRow(1, 0, scale.y, 0, 0);
m.SetRow(2, 0, 0, scale.z, 0);
m.SetRow(3, 0, 0, 0, 1.f);
return m;
}
void Matrix4x4::Decompose(Vector3 &translate, Quaternion &rotate, Vector3 &scale) const {
assert(this->IsColOrthogonal3());
Matrix3x3 r;
Decompose(translate, r, scale);
rotate = Quaternion(r);
/// Test that composing back yields the original Matrix4x4.
assert(Matrix4x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
}
void Matrix4x4::Decompose(Vector3 &translate, Matrix4x4 &rotate, Vector3 &scale) const {
assert(this->IsColOrthogonal3());
Matrix3x3 r;
Decompose(translate, r, scale);
rotate.SetRotatePart(r);
rotate.SetTranslatePart(0,0,0);
}
bool Matrix4x4::Equals(const Matrix4x4 &other, float epsilon) const {
for (int iy = 0; iy < Rows; ++iy)
for (int ix = 0; ix < Cols; ++ix)
if (!Math::EqualAbs(At(iy, ix), other[iy][ix], epsilon))
return false;
return true;
}
void
Matrix4x4::Set(float _00, float _01, float _02, float _03, float _10, float _11, float _12, float _13, float _20,
float _21, float _22, float _23, float _30, float _31, float _32, float _33) {
At(0, 0) = _00; At(0, 1) = _01; At(0, 2) = _02; At(0, 3) = _03;
At(1, 0) = _10; At(1, 1) = _11; At(1, 2) = _12; At(1, 3) = _13;
At(2, 0) = _20; At(2, 1) = _21; At(2, 2) = _22; At(2, 3) = _23;
At(3, 0) = _30; At(3, 1) = _31; At(3, 2) = _32; At(3, 3) = _33;
}
void Matrix4x4::Set(const Matrix4x4 &rhs) {
Set(rhs.ptr());
}
void Matrix4x4::Set(const float *p) {
assert(p);
Set(p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10], p[11], p[12], p[13], p[14], p[15]);
}
Matrix4x4 Matrix4x4::Adjugate() const {
Matrix4x4 a;
for(int iy = 0; iy < Rows; ++iy)
for(int ix = 0; ix < Cols; ++ix)
a[iy][ix] = (((ix+iy) & 1) != 0) ? -Minor(iy, ix) : Minor(iy, ix);
return a;
}
void Matrix4x4::SetIdentity() {
Set(1,0,0,0,
0,1,0,0,
0,0,1,0,
0,0,0,1);
}
bool Matrix4x4::InverseColOrthogonal() {
assert(IsColOrthogonal3());
float s1 = Vector3(At(0,0), At(1,0), At(2,0)).LengthSquared();
float s2 = Vector3(At(0,1), At(1,1), At(2,1)).LengthSquared();
float s3 = Vector3(At(0,2), At(1,2), At(2,2)).LengthSquared();
if (s1 < 1e-8f || s2 < 1e-8f || s3 < 1e-8f)
return false;
s1 = 1.f / s1;
s2 = 1.f / s2;
s3 = 1.f / s3;
Swap(At(0,1), At(1,0));
Swap(At(0,2), At(2,0));
Swap(At(1,2), At(2,1));
At(0,0) *= s1; At(0,1) *= s1; At(0,2) *= s1;
At(1,0) *= s2; At(1,1) *= s2; At(1,2) *= s2;
At(2,0) *= s3; At(2,1) *= s3; At(2,2) *= s3;
SetTranslatePart(TransformDir(-At(0, 3), -At(1,3), -At(2,3)));
assert(IsRowOrthogonal3());
return true;
}
void Matrix4x4::SetRotatePartX(float angleRadians) {
Set3x3PartRotateX(*this, angleRadians);
}
void Matrix4x4::SetRotatePartY(float angleRadians) {
Set3x3PartRotateY(*this, angleRadians);
}
void Matrix4x4::SetRotatePartZ(float angleRadians) {
Set3x3PartRotateZ(*this, angleRadians);
}
Matrix4x4::Matrix4x4(const Matrix3x3 &m) {
Set(m.At(0,0), m.At(0,1), m.At(0,2), 0.f,
m.At(1,0), m.At(1,1), m.At(1,2), 0.f,
m.At(2,0), m.At(2,1), m.At(2,2), 0.f,
0.f, 0.f, 0.f, 1.f);
}
Matrix4x4 Matrix4x4::RandomGeneral(RNG &rng, float minElem, float maxElem) {
assert(std::isfinite(minElem));
assert(std::isfinite(maxElem));
Matrix4x4 m;
for (int y = 0; y < 4; ++y)
for (int x = 0; x < 4; ++x)
m[y][x] = rng.Float(minElem, maxElem);
return m;
}
}
Reference in New Issue View Git Blame Copy Permalink