41 lines
1.4 KiB
C#
41 lines
1.4 KiB
C#
using System;
|
|
using System.Numerics;
|
|
using NitroxModel.DataStructures.Unity;
|
|
|
|
namespace NitroxModel.Helper;
|
|
|
|
// ReSharper disable once InconsistentNaming
|
|
public static class Matrix4x4Extension
|
|
{
|
|
public static Matrix4x4 Compose(NitroxVector3 localPosition, NitroxQuaternion localRotation, NitroxVector3 localScale)
|
|
{
|
|
Matrix4x4 translationMatrix = Matrix4x4.CreateTranslation(localPosition.X, localPosition.Y, localPosition.Z);
|
|
Matrix4x4 rotationMatrix = Matrix4x4.CreateFromQuaternion((Quaternion)localRotation);
|
|
Matrix4x4 scaleMatrix = Matrix4x4.CreateScale(localScale.X, localScale.Y, localScale.Z);
|
|
return scaleMatrix * rotationMatrix * translationMatrix;
|
|
}
|
|
|
|
public static Matrix4x4 Invert(this Matrix4x4 m)
|
|
{
|
|
Matrix4x4.Invert(m, out Matrix4x4 result);
|
|
return result;
|
|
}
|
|
|
|
public static NitroxVector3 Transform(this Matrix4x4 m, NitroxVector3 v)
|
|
{
|
|
float x = v.X * m.M11 + v.Y * m.M21 + v.Z * m.M31 + m.M41;
|
|
float y = v.X * m.M12 + v.Y * m.M22 + v.Z * m.M32 + m.M42;
|
|
float z = v.X * m.M13 + v.Y * m.M23 + v.Z * m.M33 + m.M43;
|
|
float w = v.X * m.M14 + v.Y * m.M24 + v.Z * m.M34 + m.M44;
|
|
|
|
if (w == 0)
|
|
{
|
|
throw new ArithmeticException($"Tried to divide by zero with Matrix {m} and vector {v}");
|
|
}
|
|
|
|
w = 1f / w;
|
|
|
|
return new NitroxVector3(x * w, y * w, z * w);
|
|
}
|
|
}
|