mirror of
https://github.com/space-wizards/RobustToolbox.git
synced 2026-02-15 03:30:53 +01:00
1223 lines
47 KiB
C#
1223 lines
47 KiB
C#
#region --- License ---
|
|
|
|
/*
|
|
Copyright (c) 2006 - 2008 The Open Toolkit library.
|
|
|
|
Permission is hereby granted, free of charge, to any person obtaining a copy of
|
|
this software and associated documentation files (the "Software"), to deal in
|
|
the Software without restriction, including without limitation the rights to
|
|
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
|
|
of the Software, and to permit persons to whom the Software is furnished to do
|
|
so, subject to the following conditions:
|
|
|
|
The above copyright notice and this permission notice shall be included in all
|
|
copies or substantial portions of the Software.
|
|
|
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
SOFTWARE.
|
|
*/
|
|
|
|
#endregion --- License ---
|
|
|
|
using System;
|
|
using System.Runtime.CompilerServices;
|
|
using System.Runtime.InteropServices;
|
|
|
|
namespace Robust.Shared.Maths
|
|
{
|
|
[Serializable]
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Matrix3 : IEquatable<Matrix3>, IApproxEquatable<Matrix3>
|
|
{
|
|
#region Fields & Access
|
|
|
|
/// <summary>Row 0, Column 0</summary>
|
|
public float R0C0;
|
|
|
|
/// <summary>Row 0, Column 1</summary>
|
|
public float R0C1;
|
|
|
|
/// <summary>Row 0, Column 2</summary>
|
|
public float R0C2;
|
|
|
|
/// <summary>Row 1, Column 0</summary>
|
|
public float R1C0;
|
|
|
|
/// <summary>Row 1, Column 1</summary>
|
|
public float R1C1;
|
|
|
|
/// <summary>Row 1, Column 2</summary>
|
|
public float R1C2;
|
|
|
|
/// <summary>Row 2, Column 0</summary>
|
|
public float R2C0;
|
|
|
|
/// <summary>Row 2, Column 1</summary>
|
|
public float R2C1;
|
|
|
|
/// <summary>Row 2, Column 2</summary>
|
|
public float R2C2;
|
|
|
|
/// <summary>Gets the component at the given row and column in the matrix.</summary>
|
|
/// <param name="row">The row of the matrix.</param>
|
|
/// <param name="column">The column of the matrix.</param>
|
|
/// <returns>The component at the given row and column in the matrix.</returns>
|
|
public float this[int row, int column]
|
|
{
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
get
|
|
{
|
|
switch (row)
|
|
{
|
|
case 0:
|
|
switch (column)
|
|
{
|
|
case 0: return R0C0;
|
|
case 1: return R0C1;
|
|
case 2: return R0C2;
|
|
}
|
|
|
|
break;
|
|
|
|
case 1:
|
|
switch (column)
|
|
{
|
|
case 0: return R1C0;
|
|
case 1: return R1C1;
|
|
case 2: return R1C2;
|
|
}
|
|
|
|
break;
|
|
|
|
case 2:
|
|
switch (column)
|
|
{
|
|
case 0: return R2C0;
|
|
case 1: return R2C1;
|
|
case 2: return R2C2;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
throw new IndexOutOfRangeException();
|
|
}
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
set
|
|
{
|
|
switch (row)
|
|
{
|
|
case 0:
|
|
switch (column)
|
|
{
|
|
case 0:
|
|
R0C0 = value;
|
|
return;
|
|
case 1:
|
|
R0C1 = value;
|
|
return;
|
|
case 2:
|
|
R0C2 = value;
|
|
return;
|
|
}
|
|
|
|
break;
|
|
|
|
case 1:
|
|
switch (column)
|
|
{
|
|
case 0:
|
|
R1C0 = value;
|
|
return;
|
|
case 1:
|
|
R1C1 = value;
|
|
return;
|
|
case 2:
|
|
R1C2 = value;
|
|
return;
|
|
}
|
|
|
|
break;
|
|
|
|
case 2:
|
|
switch (column)
|
|
{
|
|
case 0:
|
|
R2C0 = value;
|
|
return;
|
|
case 1:
|
|
R2C1 = value;
|
|
return;
|
|
case 2:
|
|
R2C2 = value;
|
|
return;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
throw new IndexOutOfRangeException();
|
|
}
|
|
}
|
|
|
|
/// <summary>Gets the component at the index into the matrix.</summary>
|
|
/// <param name="index">The index into the components of the matrix.</param>
|
|
/// <returns>The component at the given index into the matrix.</returns>
|
|
public float this[int index]
|
|
{
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
get
|
|
{
|
|
switch (index)
|
|
{
|
|
case 0: return R0C0;
|
|
case 1: return R0C1;
|
|
case 2: return R0C2;
|
|
case 3: return R1C0;
|
|
case 4: return R1C1;
|
|
case 5: return R1C2;
|
|
case 6: return R2C0;
|
|
case 7: return R2C1;
|
|
case 8: return R2C2;
|
|
default: throw new IndexOutOfRangeException();
|
|
}
|
|
}
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
set
|
|
{
|
|
switch (index)
|
|
{
|
|
case 0:
|
|
R0C0 = value;
|
|
return;
|
|
case 1:
|
|
R0C1 = value;
|
|
return;
|
|
case 2:
|
|
R0C2 = value;
|
|
return;
|
|
case 3:
|
|
R1C0 = value;
|
|
return;
|
|
case 4:
|
|
R1C1 = value;
|
|
return;
|
|
case 5:
|
|
R1C2 = value;
|
|
return;
|
|
case 6:
|
|
R2C0 = value;
|
|
return;
|
|
case 7:
|
|
R2C1 = value;
|
|
return;
|
|
case 8:
|
|
R2C2 = value;
|
|
return;
|
|
default: throw new IndexOutOfRangeException();
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>Converts the matrix into an array of floats.</summary>
|
|
/// <param name="matrix">The matrix to convert.</param>
|
|
/// <returns>An array of floats for the matrix.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static explicit operator float[](Matrix3 matrix)
|
|
{
|
|
return new[]
|
|
{
|
|
matrix.R0C0,
|
|
matrix.R0C1,
|
|
matrix.R0C2,
|
|
matrix.R1C0,
|
|
matrix.R1C1,
|
|
matrix.R1C2,
|
|
matrix.R2C0,
|
|
matrix.R2C1,
|
|
matrix.R2C2
|
|
};
|
|
}
|
|
|
|
#endregion Fields & Access
|
|
|
|
#region Constructors
|
|
|
|
/// <summary>Constructs left matrix with the same components as the given matrix.</summary>
|
|
/// <param name="matrix">The matrix whose components to copy.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public Matrix3(ref Matrix3 matrix)
|
|
{
|
|
R0C0 = matrix.R0C0;
|
|
R0C1 = matrix.R0C1;
|
|
R0C2 = matrix.R0C2;
|
|
R1C0 = matrix.R1C0;
|
|
R1C1 = matrix.R1C1;
|
|
R1C2 = matrix.R1C2;
|
|
R2C0 = matrix.R2C0;
|
|
R2C1 = matrix.R2C1;
|
|
R2C2 = matrix.R2C2;
|
|
}
|
|
|
|
/// <summary>Constructs left matrix with the given values.</summary>
|
|
/// <param name="r0c0">The value for row 0 column 0.</param>
|
|
/// <param name="r0c1">The value for row 0 column 1.</param>
|
|
/// <param name="r0c2">The value for row 0 column 2.</param>
|
|
/// <param name="r1c0">The value for row 1 column 0.</param>
|
|
/// <param name="r1c1">The value for row 1 column 1.</param>
|
|
/// <param name="r1c2">The value for row 1 column 2.</param>
|
|
/// <param name="r2c0">The value for row 2 column 0.</param>
|
|
/// <param name="r2c1">The value for row 2 column 1.</param>
|
|
/// <param name="r2c2">The value for row 2 column 2.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public Matrix3
|
|
(
|
|
float r0c0,
|
|
float r0c1,
|
|
float r0c2,
|
|
float r1c0,
|
|
float r1c1,
|
|
float r1c2,
|
|
float r2c0,
|
|
float r2c1,
|
|
float r2c2
|
|
)
|
|
{
|
|
R0C0 = r0c0;
|
|
R0C1 = r0c1;
|
|
R0C2 = r0c2;
|
|
R1C0 = r1c0;
|
|
R1C1 = r1c1;
|
|
R1C2 = r1c2;
|
|
R2C0 = r2c0;
|
|
R2C1 = r2c1;
|
|
R2C2 = r2c2;
|
|
}
|
|
|
|
/// <summary>Constructs left matrix from the given array of float-precision floating-point numbers.</summary>
|
|
/// <param name="floatArray">The array of floats for the components of the matrix.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public Matrix3(float[] floatArray)
|
|
{
|
|
if (floatArray == null || floatArray.GetLength(0) < 9) throw new MissingFieldException();
|
|
|
|
R0C0 = floatArray[0];
|
|
R0C1 = floatArray[1];
|
|
R0C2 = floatArray[2];
|
|
R1C0 = floatArray[3];
|
|
R1C1 = floatArray[4];
|
|
R1C2 = floatArray[5];
|
|
R2C0 = floatArray[6];
|
|
R2C1 = floatArray[7];
|
|
R2C2 = floatArray[8];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Constructs a new instance.
|
|
/// </summary>
|
|
/// <param name="matrix">A Matrix4 to take the upper-left 3x3 from.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public Matrix3(Matrix4 matrix)
|
|
{
|
|
R0C0 = matrix.Row0.X;
|
|
R0C1 = matrix.Row0.Y;
|
|
R0C2 = matrix.Row0.Z;
|
|
|
|
R1C0 = matrix.Row1.X;
|
|
R1C1 = matrix.Row1.Y;
|
|
R1C2 = matrix.Row1.Z;
|
|
|
|
R2C0 = matrix.Row2.X;
|
|
R2C1 = matrix.Row2.Y;
|
|
R2C2 = matrix.Row2.Z;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Constructs a new instance from 3 vectors to quickly synthesize affine transforms.
|
|
/// </summary>
|
|
/// <param name="x">A Vector2 for the first, conventionally X basis, vector.</param>
|
|
/// <param name="y">A Vector2 for the second, conventionally Y basis, vector.</param>
|
|
/// <param name="origin">A Vector2 for the third, conventionally origin vector.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public Matrix3(ref Vector2 x, ref Vector2 y, ref Vector2 origin)
|
|
{
|
|
R0C0 = x.X;
|
|
R0C1 = y.X;
|
|
R0C2 = origin.X;
|
|
|
|
R1C0 = x.Y;
|
|
R1C1 = y.Y;
|
|
R1C2 = origin.Y;
|
|
|
|
R2C0 = 0;
|
|
R2C1 = 0;
|
|
R2C2 = 1;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Matrix3 CreateTranslation(float x, float y)
|
|
{
|
|
var result = Identity;
|
|
|
|
/* column major
|
|
0 0 x
|
|
0 0 y
|
|
0 0 1
|
|
*/
|
|
result.R0C2 = x;
|
|
result.R1C2 = y;
|
|
return result;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Matrix3 CreateTranslation(Vector2 vector)
|
|
{
|
|
return CreateTranslation(vector.X, vector.Y);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Matrix3 CreateRotation(float angle)
|
|
{
|
|
return CreateRotation(new Angle(angle));
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Matrix3 CreateRotation(Angle angle)
|
|
{
|
|
var cos = (float) Math.Cos(angle);
|
|
var sin = (float) Math.Sin(angle);
|
|
|
|
var result = Identity;
|
|
|
|
/* column major
|
|
cos -sin 0
|
|
sin cos 0
|
|
0 0 1
|
|
*/
|
|
result.R0C0 = cos;
|
|
result.R1C0 = sin;
|
|
result.R0C1 = -sin;
|
|
result.R1C1 = cos;
|
|
return result;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Matrix3 CreateScale(float x, float y)
|
|
{
|
|
var result = Identity;
|
|
|
|
/* column major
|
|
x 0 0
|
|
0 y 0
|
|
0 0 1
|
|
*/
|
|
result.R0C0 = x;
|
|
result.R1C1 = y;
|
|
return result;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Matrix3 CreateScale(in Vector2 scale)
|
|
{
|
|
return CreateScale(scale.X, scale.Y);
|
|
}
|
|
|
|
public static Matrix3 CreateTransform(in Vector2 position, in Angle rotation, in Vector2 scale)
|
|
{
|
|
var sMat = CreateScale(in scale);
|
|
var rMat = CreateRotation(rotation);
|
|
var tMat = CreateTranslation(position);
|
|
|
|
return sMat * rMat * tMat;
|
|
}
|
|
|
|
public static Matrix3 CreateTransform(in Vector2 position, in Angle rotation)
|
|
{
|
|
var rMat = CreateRotation(rotation);
|
|
var tMat = CreateTranslation(position);
|
|
|
|
return rMat * tMat;
|
|
}
|
|
|
|
#endregion Constructors
|
|
|
|
#region Equality
|
|
|
|
/// <summary>Indicates whether the current matrix is equal to another matrix.</summary>
|
|
/// <param name="matrix">The Matrix3 structure to compare with.</param>
|
|
/// <returns>true if the current matrix is equal to the matrix parameter; otherwise, false.</returns>
|
|
public bool Equals(Matrix3 other)
|
|
{
|
|
return
|
|
R0C0 == other.R0C0 &&
|
|
R0C1 == other.R0C1 &&
|
|
R0C2 == other.R0C2 &&
|
|
R1C0 == other.R1C0 &&
|
|
R1C1 == other.R1C1 &&
|
|
R1C2 == other.R1C2 &&
|
|
R2C0 == other.R2C0 &&
|
|
R2C1 == other.R2C1 &&
|
|
R2C2 == other.R2C2;
|
|
}
|
|
|
|
/// <summary>Indicates whether the current matrix is equal to another matrix.</summary>
|
|
/// <param name="matrix">The Matrix3 structure to compare to.</param>
|
|
/// <returns>true if the current matrix is equal to the matrix parameter; otherwise, false.</returns>
|
|
public bool Equals(ref Matrix3 matrix)
|
|
{
|
|
return
|
|
R0C0 == matrix.R0C0 &&
|
|
R0C1 == matrix.R0C1 &&
|
|
R0C2 == matrix.R0C2 &&
|
|
R1C0 == matrix.R1C0 &&
|
|
R1C1 == matrix.R1C1 &&
|
|
R1C2 == matrix.R1C2 &&
|
|
R2C0 == matrix.R2C0 &&
|
|
R2C1 == matrix.R2C1 &&
|
|
R2C2 == matrix.R2C2;
|
|
}
|
|
|
|
/// <summary>Indicates whether the current matrix is equal to another matrix.</summary>
|
|
/// <param name="left">The left-hand operand.</param>
|
|
/// <param name="right">The right-hand operand.</param>
|
|
/// <returns>true if the current matrix is equal to the matrix parameter; otherwise, false.</returns>
|
|
public static bool Equals(ref Matrix3 left, ref Matrix3 right)
|
|
{
|
|
return
|
|
left.R0C0 == right.R0C0 &&
|
|
left.R0C1 == right.R0C1 &&
|
|
left.R0C2 == right.R0C2 &&
|
|
left.R1C0 == right.R1C0 &&
|
|
left.R1C1 == right.R1C1 &&
|
|
left.R1C2 == right.R1C2 &&
|
|
left.R2C0 == right.R2C0 &&
|
|
left.R2C1 == right.R2C1 &&
|
|
left.R2C2 == right.R2C2;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public bool EqualsApprox(Matrix3 other)
|
|
{
|
|
return EqualsApprox(ref other, 1.0E-6f);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public bool EqualsApprox(Matrix3 other, double tolerance)
|
|
{
|
|
return EqualsApprox(ref other, (float) tolerance);
|
|
}
|
|
|
|
/// <summary>Indicates whether the current matrix is approximately equal to another matrix.</summary>
|
|
/// <param name="matrix">The Matrix3 structure to compare with.</param>
|
|
/// <param name="tolerance">The limit below which the matrices are considered equal.</param>
|
|
/// <returns>true if the current matrix is approximately equal to the matrix parameter; otherwise, false.</returns>
|
|
public bool EqualsApprox(ref Matrix3 matrix, float tolerance)
|
|
{
|
|
return
|
|
Math.Abs(R0C0 - matrix.R0C0) <= tolerance &&
|
|
Math.Abs(R0C1 - matrix.R0C1) <= tolerance &&
|
|
Math.Abs(R0C2 - matrix.R0C2) <= tolerance &&
|
|
Math.Abs(R1C0 - matrix.R1C0) <= tolerance &&
|
|
Math.Abs(R1C1 - matrix.R1C1) <= tolerance &&
|
|
Math.Abs(R1C2 - matrix.R1C2) <= tolerance &&
|
|
Math.Abs(R2C0 - matrix.R2C0) <= tolerance &&
|
|
Math.Abs(R2C1 - matrix.R2C1) <= tolerance &&
|
|
Math.Abs(R2C2 - matrix.R2C2) <= tolerance;
|
|
}
|
|
|
|
/// <summary>Indicates whether the current matrix is approximately equal to another matrix.</summary>
|
|
/// <param name="left">The left-hand operand.</param>
|
|
/// <param name="right">The right-hand operand.</param>
|
|
/// <param name="tolerance">The limit below which the matrices are considered equal.</param>
|
|
/// <returns>true if the current matrix is approximately equal to the matrix parameter; otherwise, false.</returns>
|
|
public static bool EqualsApprox(ref Matrix3 left, ref Matrix3 right, float tolerance)
|
|
{
|
|
return
|
|
Math.Abs(left.R0C0 - right.R0C0) <= tolerance &&
|
|
Math.Abs(left.R0C1 - right.R0C1) <= tolerance &&
|
|
Math.Abs(left.R0C2 - right.R0C2) <= tolerance &&
|
|
Math.Abs(left.R1C0 - right.R1C0) <= tolerance &&
|
|
Math.Abs(left.R1C1 - right.R1C1) <= tolerance &&
|
|
Math.Abs(left.R1C2 - right.R1C2) <= tolerance &&
|
|
Math.Abs(left.R2C0 - right.R2C0) <= tolerance &&
|
|
Math.Abs(left.R2C1 - right.R2C1) <= tolerance &&
|
|
Math.Abs(left.R2C2 - right.R2C2) <= tolerance;
|
|
}
|
|
|
|
#endregion Equality
|
|
|
|
#region Arithmetic Operators
|
|
|
|
/// <summary>Add left matrix to this matrix.</summary>
|
|
/// <param name="matrix">The matrix to add.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Add(ref Matrix3 matrix)
|
|
{
|
|
R0C0 = R0C0 + matrix.R0C0;
|
|
R0C1 = R0C1 + matrix.R0C1;
|
|
R0C2 = R0C2 + matrix.R0C2;
|
|
R1C0 = R1C0 + matrix.R1C0;
|
|
R1C1 = R1C1 + matrix.R1C1;
|
|
R1C2 = R1C2 + matrix.R1C2;
|
|
R2C0 = R2C0 + matrix.R2C0;
|
|
R2C1 = R2C1 + matrix.R2C1;
|
|
R2C2 = R2C2 + matrix.R2C2;
|
|
}
|
|
|
|
/// <summary>Add left matrix to this matrix.</summary>
|
|
/// <param name="matrix">The matrix to add.</param>
|
|
/// <param name="result">The resulting matrix of the addition.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Add(ref Matrix3 matrix, out Matrix3 result)
|
|
{
|
|
result.R0C0 = R0C0 + matrix.R0C0;
|
|
result.R0C1 = R0C1 + matrix.R0C1;
|
|
result.R0C2 = R0C2 + matrix.R0C2;
|
|
result.R1C0 = R1C0 + matrix.R1C0;
|
|
result.R1C1 = R1C1 + matrix.R1C1;
|
|
result.R1C2 = R1C2 + matrix.R1C2;
|
|
result.R2C0 = R2C0 + matrix.R2C0;
|
|
result.R2C1 = R2C1 + matrix.R2C1;
|
|
result.R2C2 = R2C2 + matrix.R2C2;
|
|
}
|
|
|
|
/// <summary>Add left matrix to left matrix.</summary>
|
|
/// <param name="matrix">The matrix on the matrix side of the equation.</param>
|
|
/// <param name="right">The matrix on the right side of the equation</param>
|
|
/// <param name="result">The resulting matrix of the addition.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Add(ref Matrix3 left, ref Matrix3 right, out Matrix3 result)
|
|
{
|
|
result.R0C0 = left.R0C0 + right.R0C0;
|
|
result.R0C1 = left.R0C1 + right.R0C1;
|
|
result.R0C2 = left.R0C2 + right.R0C2;
|
|
result.R1C0 = left.R1C0 + right.R1C0;
|
|
result.R1C1 = left.R1C1 + right.R1C1;
|
|
result.R1C2 = left.R1C2 + right.R1C2;
|
|
result.R2C0 = left.R2C0 + right.R2C0;
|
|
result.R2C1 = left.R2C1 + right.R2C1;
|
|
result.R2C2 = left.R2C2 + right.R2C2;
|
|
}
|
|
|
|
/// <summary>Subtract matrix from this matrix.</summary>
|
|
/// <param name="matrix">The matrix to subtract.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Subtract(ref Matrix3 matrix)
|
|
{
|
|
R0C0 = R0C0 - matrix.R0C0;
|
|
R0C1 = R0C1 - matrix.R0C1;
|
|
R0C2 = R0C2 - matrix.R0C2;
|
|
R1C0 = R1C0 - matrix.R1C0;
|
|
R1C1 = R1C1 - matrix.R1C1;
|
|
R1C2 = R1C2 - matrix.R1C2;
|
|
R2C0 = R2C0 - matrix.R2C0;
|
|
R2C1 = R2C1 - matrix.R2C1;
|
|
R2C2 = R2C2 - matrix.R2C2;
|
|
}
|
|
|
|
/// <summary>Subtract matrix from this matrix.</summary>
|
|
/// <param name="matrix">The matrix to subtract.</param>
|
|
/// <param name="result">The resulting matrix of the subtraction.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Subtract(ref Matrix3 matrix, out Matrix3 result)
|
|
{
|
|
result.R0C0 = R0C0 - matrix.R0C0;
|
|
result.R0C1 = R0C1 - matrix.R0C1;
|
|
result.R0C2 = R0C2 - matrix.R0C2;
|
|
result.R1C0 = R1C0 - matrix.R1C0;
|
|
result.R1C1 = R1C1 - matrix.R1C1;
|
|
result.R1C2 = R1C2 - matrix.R1C2;
|
|
result.R2C0 = R2C0 - matrix.R2C0;
|
|
result.R2C1 = R2C1 - matrix.R2C1;
|
|
result.R2C2 = R2C2 - matrix.R2C2;
|
|
}
|
|
|
|
/// <summary>Subtract right matrix from left matrix.</summary>
|
|
/// <param name="left">The matrix on the matrix side of the equation.</param>
|
|
/// <param name="right">The matrix on the right side of the equation</param>
|
|
/// <param name="result">The resulting matrix of the subtraction.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Subtract(ref Matrix3 left, ref Matrix3 right, out Matrix3 result)
|
|
{
|
|
result.R0C0 = left.R0C0 - right.R0C0;
|
|
result.R0C1 = left.R0C1 - right.R0C1;
|
|
result.R0C2 = left.R0C2 - right.R0C2;
|
|
result.R1C0 = left.R1C0 - right.R1C0;
|
|
result.R1C1 = left.R1C1 - right.R1C1;
|
|
result.R1C2 = left.R1C2 - right.R1C2;
|
|
result.R2C0 = left.R2C0 - right.R2C0;
|
|
result.R2C1 = left.R2C1 - right.R2C1;
|
|
result.R2C2 = left.R2C2 - right.R2C2;
|
|
}
|
|
|
|
/// <summary>Multiply left matrix times this matrix.</summary>
|
|
/// <param name="matrix">The matrix to multiply.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Multiply(in Matrix3 matrix)
|
|
{
|
|
var r0c0 = matrix.R0C0 * R0C0 + matrix.R0C1 * R1C0 + matrix.R0C2 * R2C0;
|
|
var r0c1 = matrix.R0C0 * R0C1 + matrix.R0C1 * R1C1 + matrix.R0C2 * R2C1;
|
|
var r0c2 = matrix.R0C0 * R0C2 + matrix.R0C1 * R1C2 + matrix.R0C2 * R2C2;
|
|
|
|
var r1c0 = matrix.R1C0 * R0C0 + matrix.R1C1 * R1C0 + matrix.R1C2 * R2C0;
|
|
var r1c1 = matrix.R1C0 * R0C1 + matrix.R1C1 * R1C1 + matrix.R1C2 * R2C1;
|
|
var r1c2 = matrix.R1C0 * R0C2 + matrix.R1C1 * R1C2 + matrix.R1C2 * R2C2;
|
|
|
|
R2C0 = matrix.R2C0 * R0C0 + matrix.R2C1 * R1C0 + matrix.R2C2 * R2C0;
|
|
R2C1 = matrix.R2C0 * R0C1 + matrix.R2C1 * R1C1 + matrix.R2C2 * R2C1;
|
|
R2C2 = matrix.R2C0 * R0C2 + matrix.R2C1 * R1C2 + matrix.R2C2 * R2C2;
|
|
|
|
R0C0 = r0c0;
|
|
R0C1 = r0c1;
|
|
R0C2 = r0c2;
|
|
|
|
R1C0 = r1c0;
|
|
R1C1 = r1c1;
|
|
R1C2 = r1c2;
|
|
}
|
|
|
|
/// <summary>Multiply matrix times this matrix.</summary>
|
|
/// <param name="matrix">The matrix to multiply.</param>
|
|
/// <param name="result">The resulting matrix of the multiplication.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Multiply(ref Matrix3 matrix, out Matrix3 result)
|
|
{
|
|
result.R0C0 = matrix.R0C0 * R0C0 + matrix.R0C1 * R1C0 + matrix.R0C2 * R2C0;
|
|
result.R0C1 = matrix.R0C0 * R0C1 + matrix.R0C1 * R1C1 + matrix.R0C2 * R2C1;
|
|
result.R0C2 = matrix.R0C0 * R0C2 + matrix.R0C1 * R1C2 + matrix.R0C2 * R2C2;
|
|
result.R1C0 = matrix.R1C0 * R0C0 + matrix.R1C1 * R1C0 + matrix.R1C2 * R2C0;
|
|
result.R1C1 = matrix.R1C0 * R0C1 + matrix.R1C1 * R1C1 + matrix.R1C2 * R2C1;
|
|
result.R1C2 = matrix.R1C0 * R0C2 + matrix.R1C1 * R1C2 + matrix.R1C2 * R2C2;
|
|
result.R2C0 = matrix.R2C0 * R0C0 + matrix.R2C1 * R1C0 + matrix.R2C2 * R2C0;
|
|
result.R2C1 = matrix.R2C0 * R0C1 + matrix.R2C1 * R1C1 + matrix.R2C2 * R2C1;
|
|
result.R2C2 = matrix.R2C0 * R0C2 + matrix.R2C1 * R1C2 + matrix.R2C2 * R2C2;
|
|
}
|
|
|
|
/// <summary>Multiply left matrix times right matrix.</summary>
|
|
/// <param name="left">The matrix on the matrix side of the equation.</param>
|
|
/// <param name="right">The matrix on the right side of the equation</param>
|
|
/// <param name="result">The resulting matrix of the multiplication.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Multiply(ref Matrix3 left, ref Matrix3 right, out Matrix3 result)
|
|
{
|
|
result.R0C0 = right.R0C0 * left.R0C0 + right.R0C1 * left.R1C0 + right.R0C2 * left.R2C0;
|
|
result.R0C1 = right.R0C0 * left.R0C1 + right.R0C1 * left.R1C1 + right.R0C2 * left.R2C1;
|
|
result.R0C2 = right.R0C0 * left.R0C2 + right.R0C1 * left.R1C2 + right.R0C2 * left.R2C2;
|
|
result.R1C0 = right.R1C0 * left.R0C0 + right.R1C1 * left.R1C0 + right.R1C2 * left.R2C0;
|
|
result.R1C1 = right.R1C0 * left.R0C1 + right.R1C1 * left.R1C1 + right.R1C2 * left.R2C1;
|
|
result.R1C2 = right.R1C0 * left.R0C2 + right.R1C1 * left.R1C2 + right.R1C2 * left.R2C2;
|
|
result.R2C0 = right.R2C0 * left.R0C0 + right.R2C1 * left.R1C0 + right.R2C2 * left.R2C0;
|
|
result.R2C1 = right.R2C0 * left.R0C1 + right.R2C1 * left.R1C1 + right.R2C2 * left.R2C1;
|
|
result.R2C2 = right.R2C0 * left.R0C2 + right.R2C1 * left.R1C2 + right.R2C2 * left.R2C2;
|
|
}
|
|
|
|
/// <summary>Multiply matrix times this scalar.</summary>
|
|
/// <param name="scalar">The scalar to multiply.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Multiply(float scalar)
|
|
{
|
|
R0C0 = scalar * R0C0;
|
|
R0C1 = scalar * R0C1;
|
|
R0C2 = scalar * R0C2;
|
|
R1C0 = scalar * R1C0;
|
|
R1C1 = scalar * R1C1;
|
|
R1C2 = scalar * R1C2;
|
|
R2C0 = scalar * R2C0;
|
|
R2C1 = scalar * R2C1;
|
|
R2C2 = scalar * R2C2;
|
|
}
|
|
|
|
/// <summary>Multiply matrix times this matrix.</summary>
|
|
/// <param name="scalar">The scalar to multiply.</param>
|
|
/// <param name="result">The resulting matrix of the multiplication.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Multiply(float scalar, out Matrix3 result)
|
|
{
|
|
result.R0C0 = scalar * R0C0;
|
|
result.R0C1 = scalar * R0C1;
|
|
result.R0C2 = scalar * R0C2;
|
|
result.R1C0 = scalar * R1C0;
|
|
result.R1C1 = scalar * R1C1;
|
|
result.R1C2 = scalar * R1C2;
|
|
result.R2C0 = scalar * R2C0;
|
|
result.R2C1 = scalar * R2C1;
|
|
result.R2C2 = scalar * R2C2;
|
|
}
|
|
|
|
/// <summary>Multiply left matrix times left matrix.</summary>
|
|
/// <param name="matrix">The matrix on the matrix side of the equation.</param>
|
|
/// <param name="scalar">The scalar on the right side of the equation</param>
|
|
/// <param name="result">The resulting matrix of the multiplication.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Multiply(ref Matrix3 matrix, float scalar, out Matrix3 result)
|
|
{
|
|
result.R0C0 = scalar * matrix.R0C0;
|
|
result.R0C1 = scalar * matrix.R0C1;
|
|
result.R0C2 = scalar * matrix.R0C2;
|
|
result.R1C0 = scalar * matrix.R1C0;
|
|
result.R1C1 = scalar * matrix.R1C1;
|
|
result.R1C2 = scalar * matrix.R1C2;
|
|
result.R2C0 = scalar * matrix.R2C0;
|
|
result.R2C1 = scalar * matrix.R2C1;
|
|
result.R2C2 = scalar * matrix.R2C2;
|
|
}
|
|
|
|
#endregion Arithmetic Operators
|
|
|
|
#region Functions
|
|
|
|
public float Determinant
|
|
{
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
get => R0C0 * R1C1 * R2C2 - R0C0 * R1C2 * R2C1 - R0C1 * R1C0 * R2C2 + R0C2 * R1C0 * R2C1 + R0C1 * R1C2 * R2C0 - R0C2 * R1C1 * R2C0;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Transpose()
|
|
{
|
|
MathHelper.Swap(ref R0C1, ref R1C0);
|
|
MathHelper.Swap(ref R0C2, ref R2C0);
|
|
MathHelper.Swap(ref R1C2, ref R2C1);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Transpose(out Matrix3 result)
|
|
{
|
|
result.R0C0 = R0C0;
|
|
result.R0C1 = R1C0;
|
|
result.R0C2 = R2C0;
|
|
result.R1C0 = R0C1;
|
|
result.R1C1 = R1C1;
|
|
result.R1C2 = R2C1;
|
|
result.R2C0 = R0C2;
|
|
result.R2C1 = R1C2;
|
|
result.R2C2 = R2C2;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Transpose(ref Matrix3 matrix, out Matrix3 result)
|
|
{
|
|
result.R0C0 = matrix.R0C0;
|
|
result.R0C1 = matrix.R1C0;
|
|
result.R0C2 = matrix.R2C0;
|
|
result.R1C0 = matrix.R0C1;
|
|
result.R1C1 = matrix.R1C1;
|
|
result.R1C2 = matrix.R2C1;
|
|
result.R2C0 = matrix.R0C2;
|
|
result.R2C1 = matrix.R1C2;
|
|
result.R2C2 = matrix.R2C2;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate the inverse of the given matrix
|
|
/// </summary>
|
|
/// <param name="mat">The matrix to invert</param>
|
|
/// <returns>The inverse of the given matrix if it has one, or the input if it is singular</returns>
|
|
/// <exception cref="InvalidOperationException">Thrown if the Matrix4 is singular.</exception>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Matrix3 Invert(Matrix3 mat)
|
|
{
|
|
var result = new Matrix3();
|
|
mat.Invert(ref result);
|
|
return result;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Invert(ref Matrix3 minv)
|
|
{
|
|
//Credit: https://stackoverflow.com/a/18504573
|
|
|
|
var d = Determinant;
|
|
if (MathHelper.CloseToPercent(d, 0))
|
|
throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
|
|
|
|
var m = this;
|
|
|
|
// computes the inverse of a matrix m
|
|
double det = m.R0C0 * (m.R1C1 * m.R2C2 - m.R2C1 * m.R1C2) -
|
|
m.R0C1 * (m.R1C0 * m.R2C2 - m.R1C2 * m.R2C0) +
|
|
m.R0C2 * (m.R1C0 * m.R2C1 - m.R1C1 * m.R2C0);
|
|
|
|
var invdet = 1 / det;
|
|
|
|
minv.R0C0 = (float) ((m.R1C1 * m.R2C2 - m.R2C1 * m.R1C2) * invdet);
|
|
minv.R0C1 = (float) ((m.R0C2 * m.R2C1 - m.R0C1 * m.R2C2) * invdet);
|
|
minv.R0C2 = (float) ((m.R0C1 * m.R1C2 - m.R0C2 * m.R1C1) * invdet);
|
|
minv.R1C0 = (float) ((m.R1C2 * m.R2C0 - m.R1C0 * m.R2C2) * invdet);
|
|
minv.R1C1 = (float) ((m.R0C0 * m.R2C2 - m.R0C2 * m.R2C0) * invdet);
|
|
minv.R1C2 = (float) ((m.R1C0 * m.R0C2 - m.R0C0 * m.R1C2) * invdet);
|
|
minv.R2C0 = (float) ((m.R1C0 * m.R2C1 - m.R2C0 * m.R1C1) * invdet);
|
|
minv.R2C1 = (float) ((m.R2C0 * m.R0C1 - m.R0C0 * m.R2C1) * invdet);
|
|
minv.R2C2 = (float) ((m.R0C0 * m.R1C1 - m.R1C0 * m.R0C1) * invdet);
|
|
}
|
|
|
|
#endregion Functions
|
|
|
|
#region Transformation Functions
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Transform(ref Vector3 vector)
|
|
{
|
|
var x = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z;
|
|
var y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z;
|
|
vector.Z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z;
|
|
vector.X = x;
|
|
vector.Y = y;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Transform(in Matrix3 matrix, ref Vector3 vector)
|
|
{
|
|
var x = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z;
|
|
var y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z;
|
|
vector.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z;
|
|
vector.X = x;
|
|
vector.Y = y;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Transform(in Matrix3 matrix, ref Vector2 vector)
|
|
{
|
|
var vec3 = new Vector3(vector.X, vector.Y, 1);
|
|
Transform(matrix, ref vec3);
|
|
vector = vec3.Xy;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public Vector2 Transform(Vector2 vector)
|
|
{
|
|
return Transform(this, vector);
|
|
}
|
|
|
|
// TODO: These 2 are big-ass SIMD candidates. Trying to make it use the existing Box2Rotated SIMD seemed jank
|
|
// These are also gonna be called a decent amount.
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public Box2 TransformBox(in Box2Rotated box)
|
|
{
|
|
Span<Vector2> vertices = stackalloc Vector2[4];
|
|
vertices[0] = Transform(box.BottomLeft);
|
|
vertices[1] = Transform(box.BottomRight);
|
|
vertices[2] = Transform(box.TopRight);
|
|
vertices[3] = Transform(box.TopLeft);
|
|
|
|
var botLeft = vertices[0];
|
|
var topRight = vertices[0];
|
|
|
|
for (var i = 0; i < 4; i++)
|
|
{
|
|
var vertex = vertices[i];
|
|
|
|
botLeft = Vector2.ComponentMin(vertex, botLeft);
|
|
topRight = Vector2.ComponentMax(vertex, topRight);
|
|
}
|
|
|
|
return new Box2(botLeft, topRight);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public Box2 TransformBox(in Box2 box)
|
|
{
|
|
Span<Vector2> vertices = stackalloc Vector2[4];
|
|
vertices[0] = Transform(box.BottomLeft);
|
|
vertices[1] = Transform(box.BottomRight);
|
|
vertices[2] = Transform(box.TopRight);
|
|
vertices[3] = Transform(box.TopLeft);
|
|
|
|
var botLeft = vertices[0];
|
|
var topRight = vertices[0];
|
|
|
|
for (var i = 0; i < 4; i++)
|
|
{
|
|
var vertex = vertices[i];
|
|
|
|
botLeft = Vector2.ComponentMin(vertex, botLeft);
|
|
topRight = Vector2.ComponentMax(vertex, topRight);
|
|
}
|
|
|
|
return new Box2(botLeft, topRight);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Vector2 Transform(in Matrix3 matrix, Vector2 vector)
|
|
{
|
|
// TODO: Look at SIMD coz holy fak this is called a lot
|
|
|
|
var x = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2;
|
|
var y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2;
|
|
|
|
return new Vector2(x, y);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Transform(ref Vector3 vector, out Vector3 result)
|
|
{
|
|
result.X = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z;
|
|
result.Y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z;
|
|
result.Z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Post-multiplies a 3x3 matrix with a 3x1 vector.
|
|
/// </summary>
|
|
/// <param name="matrix">Matrix containing the transformation.</param>
|
|
/// <param name="vector">Input vector to transform.</param>
|
|
/// <param name="result">Transformed vector.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Transform(in Matrix3 matrix, in Vector3 vector, out Vector3 result)
|
|
{
|
|
var x = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z;
|
|
var y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z;
|
|
var z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z;
|
|
result = new Vector3(x, y, z);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Post-multiplies a 3x3 matrix with a 2x1 vector. The column-major 3x3 matrix is treated as
|
|
/// a 3x2 matrix for this calculation.
|
|
/// </summary>
|
|
/// <param name="matrix">Matrix containing the transformation.</param>
|
|
/// <param name="vector">Input vector to transform.</param>
|
|
/// <param name="result">Transformed vector.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Transform(in Matrix3 matrix, in Vector2 vector, out Vector2 result)
|
|
{
|
|
var x = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2;
|
|
var y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2;
|
|
result = new Vector2(x, y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Pre-multiples a 1x3 vector with a 3x3 matrix.
|
|
/// </summary>
|
|
/// <param name="matrix">Matrix containing the transformation.</param>
|
|
/// <param name="vector">Input vector to transform.</param>
|
|
/// <param name="result">Transformed vector.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Transform(in Vector3 vector, in Matrix3 matrix, out Vector3 result)
|
|
{
|
|
var x = (vector.X * matrix.R0C0) + (vector.Y * matrix.R1C0) + (vector.Z * matrix.R2C0);
|
|
var y = (vector.X * matrix.R0C1) + (vector.Y * matrix.R1C1) + (vector.Z * matrix.R2C1);
|
|
var z = (vector.X * matrix.R0C2) + (vector.Y * matrix.R1C2) + (vector.Z * matrix.R2C2);
|
|
result = new Vector3(x, y, z);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Pre-multiples a 1x2 vector with a 3x3 matrix. The row-major 3x3 matrix is treated as
|
|
/// a 2x3 matrix for this calculation.
|
|
/// </summary>
|
|
/// <param name="matrix">Matrix containing the transformation.</param>
|
|
/// <param name="vector">Input vector to transform.</param>
|
|
/// <param name="result">Transformed vector.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Transform(in Vector2 vector, in Matrix3 matrix, out Vector2 result)
|
|
{
|
|
var x = (vector.X * matrix.R0C0) + (vector.Y * matrix.R1C0) + (matrix.R2C0);
|
|
var y = (vector.X * matrix.R0C1) + (vector.Y * matrix.R1C1) + (matrix.R2C1);
|
|
result = new Vector2(x, y);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Rotate(float angle)
|
|
{
|
|
var angleRadians = MathHelper.DegreesToRadians(angle);
|
|
var sin = (float) Math.Sin(angleRadians);
|
|
var cos = (float) Math.Cos(angleRadians);
|
|
|
|
var r0c0 = cos * R0C0 + sin * R1C0;
|
|
var r0c1 = cos * R0C1 + sin * R1C1;
|
|
var r0c2 = cos * R0C2 + sin * R1C2;
|
|
|
|
R1C0 = cos * R1C0 - sin * R0C0;
|
|
R1C1 = cos * R1C1 - sin * R0C1;
|
|
R1C2 = cos * R1C2 - sin * R0C2;
|
|
|
|
R0C0 = r0c0;
|
|
R0C1 = r0c1;
|
|
R0C2 = r0c2;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Rotate(Angle angle)
|
|
{
|
|
var sin = (float) Math.Sin(angle);
|
|
var cos = (float) Math.Cos(angle);
|
|
|
|
var r0c0 = cos * R0C0 + sin * R1C0;
|
|
var r0c1 = cos * R0C1 + sin * R1C1;
|
|
var r0c2 = cos * R0C2 + sin * R1C2;
|
|
|
|
R1C0 = cos * R1C0 - sin * R0C0;
|
|
R1C1 = cos * R1C1 - sin * R0C1;
|
|
R1C2 = cos * R1C2 - sin * R0C2;
|
|
|
|
R0C0 = r0c0;
|
|
R0C1 = r0c1;
|
|
R0C2 = r0c2;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public void Rotate(float angle, out Matrix3 result)
|
|
{
|
|
var angleRadians = MathHelper.DegreesToRadians(angle);
|
|
var sin = (float) Math.Sin(angleRadians);
|
|
var cos = (float) Math.Cos(angleRadians);
|
|
|
|
result.R0C0 = cos * R0C0 + sin * R1C0;
|
|
result.R0C1 = cos * R0C1 + sin * R1C1;
|
|
result.R0C2 = cos * R0C2 + sin * R1C2;
|
|
result.R1C0 = cos * R1C0 - sin * R0C0;
|
|
result.R1C1 = cos * R1C1 - sin * R0C1;
|
|
result.R1C2 = cos * R1C2 - sin * R0C2;
|
|
result.R2C0 = R2C0;
|
|
result.R2C1 = R2C1;
|
|
result.R2C2 = R2C2;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void Rotate(ref Matrix3 matrix, float angle, out Matrix3 result)
|
|
{
|
|
var angleRadians = MathHelper.DegreesToRadians(angle);
|
|
var sin = (float) Math.Sin(angleRadians);
|
|
var cos = (float) Math.Cos(angleRadians);
|
|
|
|
result.R0C0 = cos * matrix.R0C0 + sin * matrix.R1C0;
|
|
result.R0C1 = cos * matrix.R0C1 + sin * matrix.R1C1;
|
|
result.R0C2 = cos * matrix.R0C2 + sin * matrix.R1C2;
|
|
result.R1C0 = cos * matrix.R1C0 - sin * matrix.R0C0;
|
|
result.R1C1 = cos * matrix.R1C1 - sin * matrix.R0C1;
|
|
result.R1C2 = cos * matrix.R1C2 - sin * matrix.R0C2;
|
|
result.R2C0 = matrix.R2C0;
|
|
result.R2C1 = matrix.R2C1;
|
|
result.R2C2 = matrix.R2C2;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static void RotateMatrix(float angle, out Matrix3 result)
|
|
{
|
|
var angleRadians = MathHelper.DegreesToRadians(angle);
|
|
var sin = (float) Math.Sin(angleRadians);
|
|
var cos = (float) Math.Cos(angleRadians);
|
|
|
|
result.R0C0 = cos;
|
|
result.R0C1 = sin;
|
|
result.R0C2 = 0;
|
|
result.R1C0 = -sin;
|
|
result.R1C1 = cos;
|
|
result.R1C2 = 0;
|
|
result.R2C0 = 0;
|
|
result.R2C1 = 0;
|
|
result.R2C2 = 1;
|
|
}
|
|
|
|
#endregion Transformation Functions
|
|
|
|
#region Operator Overloads
|
|
|
|
/// <summary>
|
|
/// Post-multiplies a 3x3 matrix with a 3x1 vector.
|
|
/// </summary>
|
|
/// <param name="matrix">Matrix containing the transformation.</param>
|
|
/// <param name="vector">Input vector to transform.</param>
|
|
/// <returns>Transformed vector.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Vector3 operator *(in Matrix3 matrix, in Vector3 vector)
|
|
{
|
|
Transform(in matrix, in vector, out var result);
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Post-multiplies a 3x3 matrix with a 2x1 vector. The 3x3 matrix is treated as
|
|
/// a 3x2 matrix for this calculation.
|
|
/// </summary>
|
|
/// <param name="matrix">Matrix containing the transformation.</param>
|
|
/// <param name="vector">Input vector to transform.</param>
|
|
/// <returns>Transformed vector.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Vector2 operator *(in Matrix3 matrix, in Vector2 vector)
|
|
{
|
|
Transform(in matrix, in vector, out var result);
|
|
return result;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Vector3 operator *(in Vector3 vector, in Matrix3 matrix)
|
|
{
|
|
Transform(in vector, in matrix, out var result);
|
|
return result;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Vector2 operator *(in Vector2 vector, in Matrix3 matrix)
|
|
{
|
|
Transform(in vector, in matrix, out var result);
|
|
return result;
|
|
}
|
|
|
|
/// <summary>Multiply left matrix times right matrix.</summary>
|
|
/// <param name="left">The matrix on the matrix side of the equation.</param>
|
|
/// <param name="right">The matrix on the right side of the equation</param>
|
|
/// <returns>The resulting matrix of the multiplication.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static Matrix3 operator *(Matrix3 left, Matrix3 right)
|
|
{
|
|
Multiply(ref left, ref right, out var result);
|
|
return result;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Constants
|
|
|
|
/// <summary>The identity matrix.</summary>
|
|
public static readonly Matrix3 Identity = new(
|
|
1, 0, 0,
|
|
0, 1, 0,
|
|
0, 0, 1
|
|
);
|
|
|
|
/// <summary>A matrix of all zeros.</summary>
|
|
public static readonly Matrix3 Zero = new(
|
|
0, 0, 0,
|
|
0, 0, 0,
|
|
0, 0, 0
|
|
);
|
|
|
|
#endregion Constants
|
|
|
|
#region HashCode
|
|
|
|
/// <summary>Returns the hash code for this instance.</summary>
|
|
/// <returns>A 32-bit signed integer that is the hash code for this instance.</returns>
|
|
public override int GetHashCode()
|
|
{
|
|
return
|
|
R0C0.GetHashCode() ^ R0C1.GetHashCode() ^ R0C2.GetHashCode() ^
|
|
R1C0.GetHashCode() ^ R1C1.GetHashCode() ^ R1C2.GetHashCode() ^
|
|
R2C0.GetHashCode() ^ R2C1.GetHashCode() ^ R2C2.GetHashCode();
|
|
}
|
|
|
|
#endregion HashCode
|
|
|
|
#region String
|
|
|
|
/// <summary>Returns the fully qualified type name of this instance.</summary>
|
|
/// <returns>A System.String containing left fully qualified type name.</returns>
|
|
public override string ToString()
|
|
{
|
|
return $"|{R0C0}, {R0C1}, {R0C2}|\n"
|
|
+ $"|{R1C0}, {R1C1}, {R1C2}|\n"
|
|
+ $"|{R2C0}, {R2C1}, {R2C2}|\n";
|
|
}
|
|
|
|
#endregion String
|
|
}
|
|
#pragma warning restore 3019
|
|
}
|