238 lines
8.0 KiB
C++
238 lines
8.0 KiB
C++
#pragma once
|
|
|
|
/// Template Parameterized (Generic) Matrix Functions.
|
|
|
|
#include <J3ML/LinearAlgebra/Quaternion.hpp>
|
|
#include "J3ML/J3ML.hpp"
|
|
|
|
|
|
namespace J3ML::LinearAlgebra {
|
|
|
|
template <typename Matrix>
|
|
bool InverseMatrix(Matrix &mat, float epsilon)
|
|
{
|
|
Matrix inversed = Matrix::Identity;
|
|
|
|
const int nc = std::min<int>(Matrix::Rows, Matrix::Cols);
|
|
|
|
for (int column = 0; column < nc; ++column)
|
|
{
|
|
// find the row i with i >= j such that M has the largest absolute value.
|
|
int greatest = column;
|
|
float greatestVal = std::abs(mat[greatest][column]);
|
|
for (int i = column+1; i < Matrix::Rows; i++)
|
|
{
|
|
float val = std::abs(mat[i][column]);
|
|
if (val > greatestVal) {
|
|
greatest = i;
|
|
greatestVal = val;
|
|
}
|
|
}
|
|
|
|
if (greatestVal < epsilon) {
|
|
mat = inversed;
|
|
return false;
|
|
}
|
|
|
|
// exchange rows
|
|
if (greatest != column) {
|
|
inversed.SwapRows(greatest, column);
|
|
mat.SwapRows(greatest, column);
|
|
}
|
|
|
|
// multiply rows
|
|
assert(!Math::EqualAbs(mat[column][column], 0.f, epsilon));
|
|
float scale = 1.f / mat[column][column];
|
|
inversed.ScaleRow(column, scale);
|
|
mat.ScaleRow(column, scale);
|
|
|
|
// add rows
|
|
for (int i = 0; i < column; i++) {
|
|
inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
|
|
mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
|
|
}
|
|
|
|
for (int i = column + 1; i < Matrix::Rows; i++) {
|
|
inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
|
|
mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
|
|
}
|
|
}
|
|
mat = inversed;
|
|
return true;
|
|
}
|
|
|
|
|
|
/// Computes the LU-decomposition on the given square matrix.
|
|
/// @return True if the composition was successful, false otherwise. If the return value is false, the contents of the output matrix are unspecified.
|
|
template <typename Matrix>
|
|
bool LUDecomposeMatrix(const Matrix &mat, Matrix &lower, Matrix &upper)
|
|
{
|
|
lower = Matrix::Identity;
|
|
upper = Matrix::Zero;
|
|
|
|
for (int i = 0; i < Matrix::Rows; ++i)
|
|
{
|
|
for (int col = i; col < Matrix::Cols; ++col)
|
|
{
|
|
upper[i][col] = mat[i][col];
|
|
for (int k = 0; k < i; ++k)
|
|
upper[i][col] -= lower[i][k] * upper[k][col];
|
|
}
|
|
for (int row = i+1; row < Matrix::Rows; ++row)
|
|
{
|
|
lower[row][i] = mat[row][i];
|
|
for (int k = 0; k < i; ++k)
|
|
lower[row][i] -= lower[row][k] * upper[k][i];
|
|
if (Math::EqualAbs(upper[i][i], 0.f))
|
|
return false;
|
|
lower[row][i] /= upper[i][i];
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/// Computes the Cholesky decomposition on the given square matrix *on the real domain*.
|
|
/// @return True if successful, false otherwise. If the return value is false, the contents of the output matrix are uspecified.
|
|
template <typename Matrix>
|
|
bool CholeskyDecomposeMatrix(const Matrix &mat, Matrix& lower)
|
|
{
|
|
lower = Matrix::Zero;
|
|
for (int i = 0; i < Matrix::Rows; ++i)
|
|
{
|
|
for (int j = 0; j < i; ++i)
|
|
{
|
|
lower[i][j] = mat[i][j];
|
|
for (int k = 0; k < j; ++k)
|
|
lower[i][j] -= lower[i][j] * lower[j][k];
|
|
if (Math::EqualAbs(lower[j][j], 0.f))
|
|
return false;
|
|
lower[i][j] /= lower[j][j];
|
|
}
|
|
lower[i][i] = mat[i][i];
|
|
if (lower[i][i])
|
|
return false;
|
|
for (int k = 0; k < i; ++k)
|
|
lower[i][i] -= lower[i][k] * lower[i][k];
|
|
lower[i][i] = std::sqrt(lower[i][i]);
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
template<typename Matrix>
|
|
void SetMatrixRotatePart(Matrix &m, const Quaternion &q) {
|
|
// See https://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .
|
|
|
|
assert(q.IsNormalized(1e-3f));
|
|
const float x = q.x;
|
|
const float y = q.y;
|
|
const float z = q.z;
|
|
const float w = q.w;
|
|
m[0][0] = 1 - 2 * (y * y + z * z);
|
|
m[0][1] = 2 * (x * y - z * w);
|
|
m[0][2] = 2 * (x * y + y * w);
|
|
m[1][0] = 2 * (x * y + z * w);
|
|
m[1][1] = 1 - 2 * (x * x + z * z);
|
|
m[1][2] = 2 * (y * z - x * w);
|
|
m[2][0] = 2 * (x * z - y * w);
|
|
m[2][1] = 2 * (y * z + x * w);
|
|
m[2][2] = 1 - 2 * (x * x + y * y);
|
|
}
|
|
|
|
/** Sets the top-left 3x3 area of the matrix to the rotation matrix about the X-axis. Elements
|
|
outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
|
|
in the order M*v, and clockwise if rotated in the order v*M.
|
|
@param m The matrix to store the result.
|
|
@param angle the rotation angle in radians. */
|
|
template<typename Matrix>
|
|
void Set3x3PartRotateX(Matrix &m, float angle) {
|
|
float sinz, cosz;
|
|
sinz = Math::Sin(angle);
|
|
cosz = Math::Cos(angle);
|
|
|
|
m[0][0] = 1.f;
|
|
m[0][1] = 0.f;
|
|
m[0][2] = 0.f;
|
|
m[1][0] = 0.f;
|
|
m[1][1] = cosz;
|
|
m[1][2] = -sinz;
|
|
m[2][0] = 0.f;
|
|
m[2][1] = sinz;
|
|
m[2][2] = cosz;
|
|
}
|
|
|
|
/** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Y-axis. Elements
|
|
outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
|
|
in the order M*v, and clockwise if rotated in the order v*M.
|
|
@param m The matrix to store the result
|
|
@param angle The rotation angle in radians. */
|
|
template<typename Matrix>
|
|
void Set3x3PartRotateY(Matrix &m, float angle) {
|
|
float sinz, cosz;
|
|
sinz = Math::Sin(angle);
|
|
cosz = Math::Cos(angle);
|
|
|
|
m[0][0] = cosz;
|
|
m[0][1] = 0.f;
|
|
m[0][2] = sinz;
|
|
m[1][0] = 0.f;
|
|
m[1][1] = 1.f;
|
|
m[1][2] = 0.f;
|
|
m[2][0] = -sinz;
|
|
m[2][1] = 0.f;
|
|
m[2][2] = cosz;
|
|
}
|
|
|
|
/** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Z-axis. Elements
|
|
outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
|
|
in the order of M*v, and clockwise if rotated in the order v*M.
|
|
@param m The matrix to store the result.
|
|
@param angle The rotation angle in radians. */
|
|
template<typename Matrix>
|
|
void Set3x3PartRotateZ(Matrix &m, float angle) {
|
|
float sinz, cosz;
|
|
sinz = std::sin(angle);
|
|
cosz = std::cos(angle);
|
|
|
|
m[0][0] = cosz;
|
|
m[0][1] = -sinz;
|
|
m[0][2] = 0.f;
|
|
m[1][0] = sinz;
|
|
m[1][1] = cosz;
|
|
m[1][2] = 0.f;
|
|
m[2][0] = 0.f;
|
|
m[2][1] = 0.f;
|
|
m[2][2] = 1.f;
|
|
}
|
|
|
|
|
|
/** Computes the matrix M = R_x * R_y * R_z, where R_d is the cardinal rotation matrix
|
|
about the axis +d, rotating counterclockwise.
|
|
This function was adapted from https://www.geometrictools.com/Documentation/EulerAngles.pdf .
|
|
Parameters x y and z are the angles of rotation, in radians. */
|
|
template<typename Matrix>
|
|
void Set3x3PartRotateEulerXYZ(Matrix &m, float x, float y, float z) {
|
|
// TODO: vectorize to compute 4 sines + cosines at one time;
|
|
float cx = std::cos(x);
|
|
float sx = std::sin(x);
|
|
float cy = std::cos(y);
|
|
float sy = std::sin(y);
|
|
float cz = std::cos(z);
|
|
float sz = std::sin(z);
|
|
|
|
m[0][0] = cy * cz;
|
|
m[0][1] = -cy * sz;
|
|
m[0][2] = sy;
|
|
m[1][0] = cz * sx * sy + cx * sz;
|
|
m[1][1] = cx * cz - sx * sy * sz;
|
|
m[1][2] = -cy * sx;
|
|
m[2][0] = -cx * cz * sy + sx * sz;
|
|
m[2][1] = cz * sx + cx * sy * sz;
|
|
m[2][2] = cx * cy;
|
|
}
|
|
|
|
|
|
|
|
}
|