Skip to content

Commit

Permalink
Merge pull request #1244 from NogginBops/matrix-invert-improvement
Browse files Browse the repository at this point in the history
  • Loading branch information
@PJB3005
PJB3005 committed Mar 9, 2021
2 parents 6734491 + dd09253 commit 5b77bd9
Show file tree
Hide file tree
Showing 9 changed files with 454 additions and 418 deletions.
2 changes: 1 addition & 1 deletion src/OpenTK.Graphics/OpenTK.Graphics.csproj
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<Project Sdk="Microsoft.NET.Sdk">

<PropertyGroup>
<TargetFramework>netstandard2.1</TargetFramework>
<TargetFrameworks>netstandard2.1;netcoreapp3.1</TargetFrameworks>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
<RootNamespace>OpenTK.Graphics</RootNamespace>
</PropertyGroup>
Expand Down
9 changes: 7 additions & 2 deletions src/OpenTK.Mathematics/Matrix/Matrix2.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -551,7 +551,7 @@ public static Matrix2 Subtract(Matrix2 left, Matrix2 right)
/// <exception cref="InvalidOperationException">Thrown if the Matrix2 is singular.</exception>
public static void Invert(in Matrix2 mat, out Matrix2 result)
{
var det = mat.Determinant;
var det = (mat.Row0.X * mat.Row1.Y) - (mat.Row0.Y * mat.Row1.X);

if (det == 0)
{
Expand All @@ -560,10 +560,15 @@ public static void Invert(in Matrix2 mat, out Matrix2 result)

var invDet = 1f / det;

// Because the c# jit assumes alias for byref types we need to
// save this value as the write to result.Row0.X could change the
// value of mat.Row0.X.
var row0x = mat.Row0.X;

result.Row0.X = mat.Row1.Y * invDet;
result.Row0.Y = -mat.Row0.Y * invDet;
result.Row1.X = -mat.Row1.X * invDet;
result.Row1.Y = mat.Row0.X * invDet;
result.Row1.Y = row0x * invDet;
}

/// <summary>
Expand Down
9 changes: 7 additions & 2 deletions src/OpenTK.Mathematics/Matrix/Matrix2d.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -551,7 +551,7 @@ public static Matrix2d Subtract(Matrix2d left, Matrix2d right)
/// <exception cref="InvalidOperationException">Thrown if the Matrix2d is singular.</exception>
public static void Invert(in Matrix2d mat, out Matrix2d result)
{
var det = mat.Determinant;
var det = (mat.Row0.X * mat.Row1.Y) - (mat.Row0.Y * mat.Row1.X);

if (det == 0)
{
Expand All @@ -560,10 +560,15 @@ public static void Invert(in Matrix2d mat, out Matrix2d result)

var invDet = 1f / det;

// Because the c# jit assumes alias for byref types we need to
// save this value as the write to result.Row0.X could change the
// value of mat.Row0.X.
var row0x = mat.Row0.X;

result.Row0.X = mat.Row1.Y * invDet;
result.Row0.Y = -mat.Row0.Y * invDet;
result.Row1.X = -mat.Row1.X * invDet;
result.Row1.Y = mat.Row0.X * invDet;
result.Row1.Y = row0x * invDet;
}

/// <summary>
Expand Down
140 changes: 39 additions & 101 deletions src/OpenTK.Mathematics/Matrix/Matrix3.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -804,116 +804,54 @@ public static void Mult(in Matrix3 left, in Matrix3 right, out Matrix3 result)
/// <summary>
/// Calculate the inverse of the given matrix.
/// </summary>
/// <param name="mat">The matrix to invert.</param>
/// <param name="matrix">The matrix to invert.</param>
/// <param name="result">The inverse of the given matrix if it has one, or the input if it is singular.</param>
/// <exception cref="InvalidOperationException">Thrown if the Matrix3 is singular.</exception>
public static void Invert(in Matrix3 mat, out Matrix3 result)
public static void Invert(in Matrix3 matrix, out Matrix3 result)
{
int[] colIdx = { 0, 0, 0 };
int[] rowIdx = { 0, 0, 0 };
int[] pivotIdx = { -1, -1, -1 };

float[,] inverse =
{
{ mat.Row0.X, mat.Row0.Y, mat.Row0.Z },
{ mat.Row1.X, mat.Row1.Y, mat.Row1.Z },
{ mat.Row2.X, mat.Row2.Y, mat.Row2.Z }
};

var icol = 0;
var irow = 0;
for (var i = 0; i < 3; i++)
{
var maxPivot = 0.0f;
for (var j = 0; j < 3; j++)
{
if (pivotIdx[j] != 0)
{
for (var k = 0; k < 3; ++k)
{
if (pivotIdx[k] == -1)
{
var absVal = Math.Abs(inverse[j, k]);
if (absVal > maxPivot)
{
maxPivot = absVal;
irow = j;
icol = k;
}
}
else if (pivotIdx[k] > 0)
{
result = mat;
return;
}
}
}
}

++pivotIdx[icol];

if (irow != icol)
{
for (var k = 0; k < 3; ++k)
{
var f = inverse[irow, k];
inverse[irow, k] = inverse[icol, k];
inverse[icol, k] = f;
}
}

rowIdx[i] = irow;
colIdx[i] = icol;
// Original implementation can be found here:
// https://github.com/niswegmann/small-matrix-inverse/blob/6eac02b84ad06870692abaf828638a391548502c/invert3x3_c.h
float row0X = matrix.Row0.X, row0Y = matrix.Row0.Y, row0Z = matrix.Row0.Z;
float row1X = matrix.Row1.X, row1Y = matrix.Row1.Y, row1Z = matrix.Row1.Z;
float row2X = matrix.Row2.X, row2Y = matrix.Row2.Y, row2Z = matrix.Row2.Z;

var pivot = inverse[icol, icol];
// Compute the elements needed to calculate the determinant
// so that we can throw without writing anything to the out parameter.
float invRow0X = (+row1Y * row2Z) - (row1Z * row2Y);
float invRow1X = (-row1X * row2Z) + (row1Z * row2X);
float invRow2X = (+row1X * row2Y) - (row1Y * row2X);

if (pivot == 0.0f)
{
throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
}

var oneOverPivot = 1.0f / pivot;
inverse[icol, icol] = 1.0f;
for (var k = 0; k < 3; ++k)
{
inverse[icol, k] *= oneOverPivot;
}
// Compute determinant:
float det = (row0X * invRow0X) + (row0Y * invRow1X) + (row0Z * invRow2X);

for (var j = 0; j < 3; ++j)
{
if (icol != j)
{
var f = inverse[j, icol];
inverse[j, icol] = 0.0f;
for (var k = 0; k < 3; ++k)
{
inverse[j, k] -= inverse[icol, k] * f;
}
}
}
}

for (var j = 2; j >= 0; --j)
if (det == 0f)
{
var ir = rowIdx[j];
var ic = colIdx[j];
for (var k = 0; k < 3; ++k)
{
var f = inverse[k, ir];
inverse[k, ir] = inverse[k, ic];
inverse[k, ic] = f;
}
throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
}

result.Row0.X = inverse[0, 0];
result.Row0.Y = inverse[0, 1];
result.Row0.Z = inverse[0, 2];
result.Row1.X = inverse[1, 0];
result.Row1.Y = inverse[1, 1];
result.Row1.Z = inverse[1, 2];
result.Row2.X = inverse[2, 0];
result.Row2.Y = inverse[2, 1];
result.Row2.Z = inverse[2, 2];
// Compute adjoint:
result.Row0.X = invRow0X;
result.Row0.Y = (-row0Y * row2Z) + (row0Z * row2Y);
result.Row0.Z = (+row0Y * row1Z) - (row0Z * row1Y);
result.Row1.X = invRow1X;
result.Row1.Y = (+row0X * row2Z) - (row0Z * row2X);
result.Row1.Z = (-row0X * row1Z) + (row0Z * row1X);
result.Row2.X = invRow2X;
result.Row2.Y = (-row0X * row2Y) + (row0Y * row2X);
result.Row2.Z = (+row0X * row1Y) - (row0Y * row1X);

// Multiply adjoint with reciprocal of determinant:
det = 1.0f / det;

result.Row0.X *= det;
result.Row0.Y *= det;
result.Row0.Z *= det;
result.Row1.X *= det;
result.Row1.Y *= det;
result.Row1.Z *= det;
result.Row2.X *= det;
result.Row2.Y *= det;
result.Row2.Z *= det;
}

/// <summary>
Expand Down
140 changes: 39 additions & 101 deletions src/OpenTK.Mathematics/Matrix/Matrix3d.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -800,116 +800,54 @@ public static void Mult(in Matrix3d left, in Matrix3d right, out Matrix3d result
/// <summary>
/// Calculate the inverse of the given matrix.
/// </summary>
/// <param name="mat">The matrix to invert.</param>
/// <param name="matrix">The matrix to invert.</param>
/// <param name="result">The inverse of the given matrix if it has one, or the input if it is singular.</param>
/// <exception cref="InvalidOperationException">Thrown if the Matrix3d is singular.</exception>
public static void Invert(in Matrix3d mat, out Matrix3d result)
public static void Invert(in Matrix3d matrix, out Matrix3d result)
{
int[] colIdx = { 0, 0, 0 };
int[] rowIdx = { 0, 0, 0 };
int[] pivotIdx = { -1, -1, -1 };

double[,] inverse =
{
{ mat.Row0.X, mat.Row0.Y, mat.Row0.Z },
{ mat.Row1.X, mat.Row1.Y, mat.Row1.Z },
{ mat.Row2.X, mat.Row2.Y, mat.Row2.Z }
};

var icol = 0;
var irow = 0;
for (var i = 0; i < 3; i++)
{
var maxPivot = 0.0;
for (var j = 0; j < 3; j++)
{
if (pivotIdx[j] != 0)
{
for (var k = 0; k < 3; ++k)
{
if (pivotIdx[k] == -1)
{
var absVal = Math.Abs(inverse[j, k]);
if (absVal > maxPivot)
{
maxPivot = absVal;
irow = j;
icol = k;
}
}
else if (pivotIdx[k] > 0)
{
result = mat;
return;
}
}
}
}

++pivotIdx[icol];

if (irow != icol)
{
for (var k = 0; k < 3; ++k)
{
var f = inverse[irow, k];
inverse[irow, k] = inverse[icol, k];
inverse[icol, k] = f;
}
}

rowIdx[i] = irow;
colIdx[i] = icol;
// Original implementation can be found here:
// https://github.com/niswegmann/small-matrix-inverse/blob/6eac02b84ad06870692abaf828638a391548502c/invert3x3_c.h
double row0X = matrix.Row0.X, row0Y = matrix.Row0.Y, row0Z = matrix.Row0.Z;
double row1X = matrix.Row1.X, row1Y = matrix.Row1.Y, row1Z = matrix.Row1.Z;
double row2X = matrix.Row2.X, row2Y = matrix.Row2.Y, row2Z = matrix.Row2.Z;

var pivot = inverse[icol, icol];
// Compute the elements needed to calculate the determinant
// so that we can throw without writing anything to the out parameter.
double invRow0X = (+row1Y * row2Z) - (row1Z * row2Y);
double invRow1X = (-row1X * row2Z) + (row1Z * row2X);
double invRow2X = (+row1X * row2Y) - (row1Y * row2X);

if (pivot == 0.0)
{
throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
}

var oneOverPivot = 1.0 / pivot;
inverse[icol, icol] = 1.0;
for (var k = 0; k < 3; ++k)
{
inverse[icol, k] *= oneOverPivot;
}
// Compute determinant:
double det = (row0X * invRow0X) + (row0Y * invRow1X) + (row0Z * invRow2X);

for (var j = 0; j < 3; ++j)
{
if (icol != j)
{
var f = inverse[j, icol];
inverse[j, icol] = 0.0;
for (var k = 0; k < 3; ++k)
{
inverse[j, k] -= inverse[icol, k] * f;
}
}
}
}

for (var j = 2; j >= 0; --j)
if (det == 0f)
{
var ir = rowIdx[j];
var ic = colIdx[j];
for (var k = 0; k < 3; ++k)
{
var f = inverse[k, ir];
inverse[k, ir] = inverse[k, ic];
inverse[k, ic] = f;
}
throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
}

result.Row0.X = inverse[0, 0];
result.Row0.Y = inverse[0, 1];
result.Row0.Z = inverse[0, 2];
result.Row1.X = inverse[1, 0];
result.Row1.Y = inverse[1, 1];
result.Row1.Z = inverse[1, 2];
result.Row2.X = inverse[2, 0];
result.Row2.Y = inverse[2, 1];
result.Row2.Z = inverse[2, 2];
// Compute adjoint:
result.Row0.X = invRow0X;
result.Row0.Y = (-row0Y * row2Z) + (row0Z * row2Y);
result.Row0.Z = (+row0Y * row1Z) - (row0Z * row1Y);
result.Row1.X = invRow1X;
result.Row1.Y = (+row0X * row2Z) - (row0Z * row2X);
result.Row1.Z = (-row0X * row1Z) + (row0Z * row1X);
result.Row2.X = invRow2X;
result.Row2.Y = (-row0X * row2Y) + (row0Y * row2X);
result.Row2.Z = (+row0X * row1Y) - (row0Y * row1X);

// Multiply adjoint with reciprocal of determinant:
det = 1.0f / det;

result.Row0.X *= det;
result.Row0.Y *= det;
result.Row0.Z *= det;
result.Row1.X *= det;
result.Row1.Y *= det;
result.Row1.Z *= det;
result.Row2.X *= det;
result.Row2.Y *= det;
result.Row2.Z *= det;
}

/// <summary>
Expand Down

0 comments on commit 5b77bd9

Please sign in to comment.