297 lines
10 KiB
C++
297 lines
10 KiB
C++
#include <J3ML/LinearAlgebra/Matrix3x3.h>
|
|
#include <cmath>
|
|
|
|
namespace LinearAlgebra {
|
|
|
|
const Matrix3x3 Matrix3x3::Zero = Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
const Matrix3x3 Matrix3x3::Identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
|
|
const Matrix3x3 Matrix3x3::NaN = Matrix3x3(NAN);
|
|
|
|
Matrix3x3::Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21,
|
|
float m22) {
|
|
this->elems[0][0] = m00;
|
|
this->elems[0][1] = m01;
|
|
this->elems[0][2] = m02;
|
|
|
|
this->elems[1][0] = m10;
|
|
this->elems[1][1] = m11;
|
|
this->elems[1][2] = m12;
|
|
|
|
this->elems[2][0] = m20;
|
|
this->elems[2][1] = m21;
|
|
this->elems[2][2] = m22;
|
|
}
|
|
|
|
Vector3 Matrix3x3::GetRow(int index) const {
|
|
float x = this->elems[index][0];
|
|
float y = this->elems[index][1];
|
|
float z = this->elems[index][2];
|
|
return {x, y, z};
|
|
}
|
|
|
|
Vector3 Matrix3x3::GetColumn(int index) const {
|
|
float x = this->elems[0][index];
|
|
float y = this->elems[1][index];
|
|
float z = this->elems[2][index];
|
|
|
|
return {x, y, z};
|
|
}
|
|
|
|
float Matrix3x3::At(int x, int y) const {
|
|
return this->elems[x][y];
|
|
|
|
}
|
|
|
|
Vector3 Matrix3x3::operator*(const Vector3 &rhs) const {
|
|
return {
|
|
At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z,
|
|
At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z,
|
|
At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z
|
|
};
|
|
}
|
|
|
|
Matrix3x3 Matrix3x3::operator*(const Matrix3x3 &rhs) const {
|
|
//Matrix3x3 r;
|
|
auto m00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
|
|
auto m01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
|
|
auto m02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);
|
|
|
|
auto m10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
|
|
auto m11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
|
|
auto m12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2);
|
|
|
|
auto m20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0);
|
|
auto m21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1);
|
|
auto m22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2);
|
|
|
|
return Matrix3x3({m00, m01, m02}, {m10, m11, m12}, {m20, m21, m22});
|
|
}
|
|
|
|
Matrix3x3::Matrix3x3(float val) {
|
|
this->elems[0][0] = val;
|
|
this->elems[0][1] = val;
|
|
this->elems[0][2] = val;
|
|
|
|
this->elems[1][0] = val;
|
|
this->elems[1][1] = val;
|
|
this->elems[1][2] = val;
|
|
|
|
this->elems[2][0] = val;
|
|
this->elems[2][1] = val;
|
|
this->elems[2][2] = val;
|
|
|
|
}
|
|
|
|
Matrix3x3::Matrix3x3(const Vector3 &r1, const Vector3 &r2, const Vector3 &r3) {
|
|
this->elems[0][0] = r1.x;
|
|
this->elems[0][1] = r1.y;
|
|
this->elems[0][2] = r1.z;
|
|
|
|
this->elems[1][0] = r2.x;
|
|
this->elems[1][1] = r2.y;
|
|
this->elems[1][2] = r2.z;
|
|
|
|
this->elems[2][0] = r3.x;
|
|
this->elems[2][1] = r3.y;
|
|
this->elems[2][2] = r3.z;
|
|
}
|
|
|
|
Matrix3x3::Matrix3x3(const Quaternion &orientation) {
|
|
|
|
}
|
|
|
|
float Matrix3x3::Determinant() 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];
|
|
|
|
// Aight, what the fuck?
|
|
return a*(e*i - f*h) + b*(f*g - d*i) + c*(d*h - e*g);
|
|
}
|
|
|
|
Matrix3x3 Matrix3x3::Inverse() const {
|
|
// Compute the inverse directly using Cramer's rule
|
|
// Warning: This method is numerically very unstable!
|
|
float d = Determinant();
|
|
|
|
d = 1.f / d;
|
|
|
|
Matrix3x3 i = {
|
|
d * (At(1, 1) * At(2, 2) - At(1, 2) * At(2, 1)),
|
|
d * (At(0, 2) * At(2, 1) - At(0, 1) * At(2, 2)),
|
|
d * (At(0, 1) * At(1, 2) - At(0, 2) * At(1, 1)),
|
|
|
|
d * (At(1, 2) * At(2, 0) - At(1, 0) * At(2, 2)),
|
|
d * (At(0, 0) * At(2, 2) - At(0, 2) * At(2, 0)),
|
|
d * (At(0, 2) * At(1, 0) - At(0, 0) * At(1, 2)),
|
|
|
|
d * (At(1, 0) * At(2, 1) - At(1, 1) * At(2, 0)),
|
|
d * (At(2, 0) * At(0, 1) - At(0, 0) * At(2,1)),
|
|
d * (At(0,0) * At(1, 1) - At(0, 1) * At(1, 0))
|
|
};
|
|
|
|
return i;
|
|
}
|
|
|
|
Matrix3x3 Matrix3x3::Transpose() const {
|
|
auto m00 = this->elems[0][0];
|
|
auto m01 = this->elems[0][1];
|
|
auto m02 = this->elems[0][2];
|
|
|
|
auto m10 = this->elems[1][0];
|
|
auto m11 = this->elems[1][1];
|
|
auto m12 = this->elems[1][2];
|
|
|
|
auto m20 = this->elems[2][0];
|
|
auto m21 = this->elems[2][1];
|
|
auto m22 = this->elems[2][2];
|
|
|
|
// NO: This is correct order for transposition!
|
|
return {
|
|
m00, m10, m20,
|
|
m01, m11, m21,
|
|
m02, m12, m22
|
|
};
|
|
}
|
|
|
|
Vector2 Matrix3x3::Transform(const Vector2 &rhs) const {
|
|
return {
|
|
At(0,0) * rhs.x + At(0, 1) * rhs.y,
|
|
At(1,0) * rhs.x + At(1, 1) * rhs.y
|
|
};
|
|
}
|
|
|
|
Vector3 Matrix3x3::Transform(const Vector3 &rhs) const {
|
|
return {
|
|
At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z,
|
|
At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z,
|
|
At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z
|
|
};
|
|
}
|
|
|
|
Quaternion Matrix3x3::ToQuat() const {
|
|
auto m00 = At(0,0);
|
|
auto m01 = At(0, 1);
|
|
auto m02 = At(0, 2);
|
|
auto m10 = At(1,0);
|
|
auto m11 = At(1, 1);
|
|
auto m12 = At(1, 2);
|
|
auto m20 = At(2,0);
|
|
auto m21 = At(2, 1);
|
|
auto m22 = At(2, 2);
|
|
|
|
auto w = std::sqrt(1.f + m00 + m11 + m22) / 2.f;
|
|
float w4 = (4.f * w);
|
|
return {
|
|
(m21 - m12) / w4,
|
|
(m02 - m20) / w4,
|
|
(m10 - m01) / w4,
|
|
w
|
|
};
|
|
}
|
|
|
|
void Matrix3x3::SetRotatePart(const Vector3 &a, float angle) {
|
|
float s = std::sin(angle);
|
|
float c = std::cos(angle);
|
|
|
|
const float c1 = 1.f - c;
|
|
|
|
elems[0][0] = c+c1*a.x*a.x;
|
|
elems[1][0] = c1*a.x*a.y+s*a.z;
|
|
elems[2][0] = c1*a.x*a.z-s*a.y;
|
|
|
|
elems[0][1] = c1*a.x*a.y-s*a.z;
|
|
elems[1][1] = c+c1*a.y*a.y;
|
|
elems[2][1] = c1*a.y*a.z+s*a.x;
|
|
|
|
elems[0][2] = c1*a.x*a.z+s*a.y;
|
|
elems[1][2] = c1*a.y*a.z-s*a.x;
|
|
elems[2][2] = c+c1*a.z*a.z;
|
|
|
|
}
|
|
|
|
Matrix3x3 Matrix3x3::RotateAxisAngle(const Vector3 &axis, float angleRadians) {
|
|
Matrix3x3 r;
|
|
r.SetRotatePart(axis, angleRadians);
|
|
return r;
|
|
}
|
|
|
|
void Matrix3x3::SetRow(int i, const Vector3 &vec) {
|
|
elems[i][0] = vec.x;
|
|
elems[i][1] = vec.y;
|
|
elems[i][2] = vec.z;
|
|
}
|
|
|
|
void Matrix3x3::SetColumn(int i, const Vector3& vec)
|
|
{
|
|
elems[0][i] = vec.x;
|
|
elems[1][i] = vec.y;
|
|
elems[2][i] = vec.z;
|
|
}
|
|
|
|
Matrix3x3
|
|
Matrix3x3::LookAt(const Vector3 &forward, const Vector3 &target, const Vector3 &localUp, const Vector3 &worldUp) {
|
|
// User must input proper normalized input direction vectors
|
|
// In the local space, the forward and up directions must be perpendicular to be well-formed.
|
|
// In the world space, the target direction and world up cannot be degenerate (co-linear)
|
|
// Generate the third basis vector in the local space;
|
|
Vector3 localRight = localUp.Cross(forward).Normalize();
|
|
// A. Now we have an orthonormal linear basis {localRight, localUp, forward} for the object local space.
|
|
// Generate the third basis vector for the world space
|
|
Vector3 worldRight = worldUp.Cross(target).Normalize();
|
|
// Since the input worldUp vector is not necessarily perpendicular to the target direction vector
|
|
// We need to compute the real world space up vector that the "head" of the object will point
|
|
// towards when the model is looking towards the desired target direction
|
|
Vector3 perpWorldUp = target.Cross(worldRight).Normalize();
|
|
// B. Now we have an orthonormal linear basis {worldRight, perpWorldUp, targetDirection } for the desired target orientation.
|
|
// We want to build a matrix M that performs the following mapping:
|
|
// 1. localRight must be mapped to worldRight. (M * localRight = worldRight)
|
|
// 2. localUp must be mapped to perpWorldUp. (M * localUp = perpWorldUp)
|
|
// 3. localForward must be mapped to targetDirection. (M * localForward = targetDirection)
|
|
// i.e. we want to map the basis A to basis B.
|
|
|
|
// This matrix M exists, and it is an orthonormal rotation matrix with a determinant of +1, because
|
|
// the bases A and B are orthonormal with the same handedness.
|
|
|
|
// Below, use the notation that (a,b,c) is a 3x3 matrix with a as its first column, b second, and c third.
|
|
|
|
// By algebraic manipulation, we can rewrite conditions 1, 2 and 3 in a matrix form:
|
|
// M * (localRight, localUp, localForward) = (worldRight, perpWorldUp, targetDirection)
|
|
// or M = (worldRight, perpWorldUp, targetDirection) * (localRight, localUp, localForward)^{-1}.
|
|
// or M = m1 * m2, where
|
|
|
|
// m1 equals (worldRight, perpWorldUp, target):
|
|
Matrix3x3 m1(worldRight, perpWorldUp, target);
|
|
|
|
// and m2 equals (localRight, localUp, localForward)^{-1}:
|
|
Matrix3x3 m2;
|
|
m2.SetRow(0, localRight);
|
|
m2.SetRow(1, localUp);
|
|
m2.SetRow(2, forward);
|
|
// Above we used the shortcut that for an orthonormal matrix M, M^{-1} = M^T. So set the rows
|
|
// and not the columns to directly produce the transpose, i.e. the inverse of (localRight, localUp, localForward).
|
|
|
|
// Compute final M.
|
|
m2 = m1 * m2;
|
|
|
|
// And fix any numeric stability issues by re-orthonormalizing the result.
|
|
m2.Orthonormalize(0, 1, 2);
|
|
return m2;
|
|
}
|
|
|
|
Vector3 Matrix3x3::Diagonal() const {
|
|
return {elems[0][0],
|
|
elems[1][1],
|
|
elems[2][2]
|
|
};
|
|
}
|
|
|
|
}
|
|
|