diff --git a/Assets/UniGLTF/Runtime/Utils/MathHelper.cs b/Assets/UniGLTF/Runtime/Utils/MathHelper.cs new file mode 100644 index 000000000..86cb3646b --- /dev/null +++ b/Assets/UniGLTF/Runtime/Utils/MathHelper.cs @@ -0,0 +1,89 @@ +using System.Runtime.CompilerServices; +using Unity.Mathematics; + +namespace UniGLTF.Runtime.Utils +{ + public static class MathHelper + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float3 MultiplyPoint3x4(float4x4 matrix, float3 point) + { + return math.mul(matrix, new float4(point, 1)).xyz; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float3 MultiplyPoint(float4x4 matrix, float3 point) + { + var v = math.mul(matrix, new float4(point, 1.0f)); + return v.xyz / v.w; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float3 MultiplyVector(float4x4 matrix, float3 vector) + { + return math.mul(matrix, new float4(vector, 0)).xyz; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static quaternion FromToRotation(in float3 fromVector, in float3 toVector) + { + if (math.lengthsq(fromVector) == 0 || math.lengthsq(toVector) == 0) + { + return quaternion.identity; + } + + float3 from = math.normalize(fromVector); + float3 to = math.normalize(toVector); + + var dot = math.dot(from, to); + switch(dot) + { + case >= 1.0f: + return quaternion.identity; + case <= -1.0f: + { + var axis = math.cross(from, new float3(1, 0, 0)); + if (math.lengthsq(axis) < 0.0001f) + { + axis = math.cross(from, new float3(0, 1, 0)); + } + return quaternion.AxisAngle(math.normalize(axis), math.PI); + } + default: + { + var angle = math.acos(dot); + var axis = math.cross(from, to); + return quaternion.AxisAngle(math.normalize(axis), angle); + } + } + } + + /// + /// を Unity.Mathematics パッケージ向けに再実装した関数 + /// + /// + /// + /// + public static bool Approximately(float a, float b) + { + return math.abs(b - a) < math.max(1E-06f * math.max(math.abs(a), math.abs(b)), math.EPSILON * 8f); + } + + /// + public static bool Approximately(float3 a, float3 b) + { + return Approximately(a.x, b.x) && + Approximately(a.y, b.y) && + Approximately(a.z, b.z); + } + + /// + public static bool Approximately(quaternion a, quaternion b) + { + return Approximately(a.value.x, b.value.x) && + Approximately(a.value.y, b.value.y) && + Approximately(a.value.z, b.value.z) && + Approximately(a.value.w, b.value.w); + } + } +} diff --git a/Assets/UniGLTF/Runtime/Utils/MathHelper.cs.meta b/Assets/UniGLTF/Runtime/Utils/MathHelper.cs.meta new file mode 100644 index 000000000..4fa53e41b --- /dev/null +++ b/Assets/UniGLTF/Runtime/Utils/MathHelper.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: 50ff9ccb88bd4c208ab432127ec7b04d +timeCreated: 1751330089 \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/Utils/UniGLTF.Utils.asmdef b/Assets/UniGLTF/Runtime/Utils/UniGLTF.Utils.asmdef index 57d5bdce8..682841075 100644 --- a/Assets/UniGLTF/Runtime/Utils/UniGLTF.Utils.asmdef +++ b/Assets/UniGLTF/Runtime/Utils/UniGLTF.Utils.asmdef @@ -1,7 +1,9 @@ { "name": "UniGLTF.Utils", "rootNamespace": "", - "references": [], + "references": [ + "GUID:d8b63aba1907145bea998dd612889d6b" + ], "includePlatforms": [], "excludePlatforms": [], "allowUnsafeCode": false, diff --git a/Assets/UniGLTF/Tests/UniGLTF.Tests.asmdef b/Assets/UniGLTF/Tests/UniGLTF.Tests.asmdef index f3e06d294..d5dfef6ec 100644 --- a/Assets/UniGLTF/Tests/UniGLTF.Tests.asmdef +++ b/Assets/UniGLTF/Tests/UniGLTF.Tests.asmdef @@ -6,7 +6,8 @@ "GUID:5f875fdc81c40184c8333b9d63c6ddd5", "GUID:27619889b8ba8c24980f49ee34dbb44a", "GUID:0acc523941302664db1f4e527237feb3", - "GUID:1cd941934d098654fa21a13f28346412" + "GUID:1cd941934d098654fa21a13f28346412", + "GUID:d8b63aba1907145bea998dd612889d6b" ], "includePlatforms": [ "Editor" diff --git a/Assets/UniGLTF/Tests/UniGLTF/MathTests.cs b/Assets/UniGLTF/Tests/UniGLTF/MathTests.cs new file mode 100644 index 000000000..5c1395b64 --- /dev/null +++ b/Assets/UniGLTF/Tests/UniGLTF/MathTests.cs @@ -0,0 +1,49 @@ +using NUnit.Framework; +using UniGLTF.Runtime.Utils; +using Unity.Mathematics; +using UnityEngine; +namespace UniGLTF +{ + public class MathTests + { + private readonly float4x4 _matrix = new float4x4( + 1, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16); + private readonly float3 _vector1 = new float3(1, 2, 3); + private readonly float3 _vector2 = new float3(4, 5, 6); + + [Test] + public void MultiplyPoint3x4Test() + { + var result = MathHelper.MultiplyPoint3x4(_matrix, _vector1); + var expected = ((Matrix4x4)_matrix).MultiplyPoint3x4(_vector1); + Assert.That(MathHelper.Approximately(result, expected), Is.True); + } + + [Test] + public void MultiplyPointTest() + { + var result = MathHelper.MultiplyPoint(_matrix, _vector1); + var expected = ((Matrix4x4)_matrix).MultiplyPoint(_vector1); + Assert.That(MathHelper.Approximately(result, expected), Is.True); + } + + [Test] + public void MultiplyVectorTest() + { + var result = MathHelper.MultiplyVector(_matrix, _vector1); + var expected = ((Matrix4x4)_matrix).MultiplyVector(_vector1); + Assert.That(MathHelper.Approximately(result, expected), Is.True); + } + + [Test] + public void FromToRotationTest() + { + var result = MathHelper.FromToRotation(_vector1, _vector2); + var expected = Quaternion.FromToRotation(_vector1, _vector2); + Assert.That(MathHelper.Approximately(result, expected), Is.True); + } + } +} diff --git a/Assets/UniGLTF/Tests/UniGLTF/MathTests.cs.meta b/Assets/UniGLTF/Tests/UniGLTF/MathTests.cs.meta new file mode 100644 index 000000000..33e86dc0f --- /dev/null +++ b/Assets/UniGLTF/Tests/UniGLTF/MathTests.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: 4b920c4a4ea1401085ba8923e4ccaca2 +timeCreated: 1751329860 \ No newline at end of file