diff --git a/src/DotRecast.Core/Numerics/RcVec3f.cs b/src/DotRecast.Core/Numerics/RcVec3f.cs index 2335816..07aac41 100644 --- a/src/DotRecast.Core/Numerics/RcVec3f.cs +++ b/src/DotRecast.Core/Numerics/RcVec3f.cs @@ -259,16 +259,6 @@ namespace DotRecast.Core.Numerics return right * left; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static RcVec3f Cross(RcVec3f v1, RcVec3f v2) - { - return new RcVec3f( - (v1.Y * v2.Z) - (v1.Z * v2.Y), - (v1.Z * v2.X) - (v1.X * v2.Z), - (v1.X * v2.Y) - (v1.Y * v2.X) - ); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static RcVec3f Lerp(RcVec3f v1, RcVec3f v2, float t) { @@ -505,21 +495,14 @@ namespace DotRecast.Core.Numerics e0.Z = i.Z - verts[j + 2]; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Cross(float[] dest, float[] v1, float[] v2) + public static RcVec3f Cross(RcVec3f v1, RcVec3f v2) { - dest[0] = v1[1] * v2[2] - v1[2] * v2[1]; - dest[1] = v1[2] * v2[0] - v1[0] * v2[2]; - dest[2] = v1[0] * v2[1] - v1[1] * v2[0]; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Cross(ref RcVec3f dest, RcVec3f v1, RcVec3f v2) - { - dest.X = v1.Y * v2.Z - v1.Z * v2.Y; - dest.Y = v1.Z * v2.X - v1.X * v2.Z; - dest.Z = v1.X * v2.Y - v1.Y * v2.X; + return new RcVec3f( + (v1.Y * v2.Z) - (v1.Z * v2.Y), + (v1.Z * v2.X) - (v1.X * v2.Z), + (v1.X * v2.Y) - (v1.Y * v2.X) + ); } /// Normalizes the vector. diff --git a/src/DotRecast.Core/Numerics/RcVecExtensions.cs b/src/DotRecast.Core/Numerics/RcVecUtils.cs similarity index 83% rename from src/DotRecast.Core/Numerics/RcVecExtensions.cs rename to src/DotRecast.Core/Numerics/RcVecUtils.cs index 1cd2952..7f12d90 100644 --- a/src/DotRecast.Core/Numerics/RcVecExtensions.cs +++ b/src/DotRecast.Core/Numerics/RcVecUtils.cs @@ -1,10 +1,9 @@ using System; using System.Runtime.CompilerServices; -using DotRecast.Core.Numerics; namespace DotRecast.Core.Numerics { - public static class RcVecExtensions + public static class RcVecUtils { [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Get(this RcVec2f v, int i) @@ -56,5 +55,13 @@ namespace DotRecast.Core.Numerics return @this.X * v[vi] + @this.Z * v[vi + 2]; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Cross(float[] dest, float[] v1, float[] v2) + { + dest[0] = v1[1] * v2[2] - v1[2] * v2[1]; + dest[1] = v1[2] * v2[0] - v1[0] * v2[2]; + dest[2] = v1[0] * v2[1] - v1[1] * v2[0]; + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Dynamic/Colliders/DtBoxCollider.cs b/src/DotRecast.Detour.Dynamic/Colliders/DtBoxCollider.cs index 517c198..b7c84b3 100644 --- a/src/DotRecast.Detour.Dynamic/Colliders/DtBoxCollider.cs +++ b/src/DotRecast.Detour.Dynamic/Colliders/DtBoxCollider.cs @@ -76,11 +76,11 @@ namespace DotRecast.Detour.Dynamic.Colliders new RcVec3f(up.X, up.Y, up.Z), RcVec3f.Zero }; - + halfEdges[1] = RcVec3f.Normalize(halfEdges[1]); - RcVec3f.Cross(ref halfEdges[0], up, forward); + halfEdges[0] = RcVec3f.Cross(up, forward); halfEdges[0] = RcVec3f.Normalize(halfEdges[0]); - RcVec3f.Cross(ref halfEdges[2], halfEdges[0], up); + halfEdges[2] = RcVec3f.Cross(halfEdges[0], up); halfEdges[2] = RcVec3f.Normalize(halfEdges[2]); halfEdges[0].X *= extent.X; halfEdges[0].Y *= extent.X; diff --git a/src/DotRecast.Recast.Toolset/Gizmos/RcCapsuleGizmo.cs b/src/DotRecast.Recast.Toolset/Gizmos/RcCapsuleGizmo.cs index 7736529..90a10a7 100644 --- a/src/DotRecast.Recast.Toolset/Gizmos/RcCapsuleGizmo.cs +++ b/src/DotRecast.Recast.Toolset/Gizmos/RcCapsuleGizmo.cs @@ -1,6 +1,5 @@ using System; using DotRecast.Core.Numerics; - using static DotRecast.Recast.Toolset.Gizmos.RcGizmoHelper; namespace DotRecast.Recast.Toolset.Gizmos @@ -25,7 +24,7 @@ namespace DotRecast.Recast.Toolset.Gizmos normals[1] = RcVec3f.Normalize(normals[1]); normals[0] = GetSideVector(axis); normals[2] = RcVec3f.Zero; - RcVec3f.Cross(ref normals[2], normals[0], normals[1]); + normals[2] = RcVec3f.Cross(normals[0], normals[1]); normals[2] = RcVec3f.Normalize(normals[2]); triangles = GenerateSphericalTriangles(); var trX = new RcVec3f(normals[0].X, normals[1].X, normals[2].X); @@ -55,15 +54,14 @@ namespace DotRecast.Recast.Toolset.Gizmos private RcVec3f GetSideVector(RcVec3f axis) { - RcVec3f side = new RcVec3f(1, 0, 0); + var side = new RcVec3f(1, 0, 0); if (axis.X > 0.8) { side = new RcVec3f(0, 0, 1); } - RcVec3f forward = new RcVec3f(); - RcVec3f.Cross(ref forward, side, axis); - RcVec3f.Cross(ref side, axis, forward); + var forward = RcVec3f.Cross(side, axis); + side = RcVec3f.Cross(axis, forward); side = RcVec3f.Normalize(side); return side; } diff --git a/src/DotRecast.Recast.Toolset/Gizmos/RcCylinderGizmo.cs b/src/DotRecast.Recast.Toolset/Gizmos/RcCylinderGizmo.cs index c77b46e..2183bbf 100644 --- a/src/DotRecast.Recast.Toolset/Gizmos/RcCylinderGizmo.cs +++ b/src/DotRecast.Recast.Toolset/Gizmos/RcCylinderGizmo.cs @@ -1,6 +1,5 @@ using System; using DotRecast.Core.Numerics; - using static DotRecast.Recast.Toolset.Gizmos.RcGizmoHelper; @@ -25,7 +24,7 @@ namespace DotRecast.Recast.Toolset.Gizmos normals[1] = RcVec3f.Normalize(normals[1]); normals[0] = GetSideVector(axis); normals[2] = RcVec3f.Zero; - RcVec3f.Cross(ref normals[2], normals[0], normals[1]); + normals[2] = RcVec3f.Cross(normals[0], normals[1]); normals[2] = RcVec3f.Normalize(normals[2]); triangles = GenerateCylindricalTriangles(); RcVec3f trX = new RcVec3f(normals[0].X, normals[1].X, normals[2].X); @@ -67,9 +66,8 @@ namespace DotRecast.Recast.Toolset.Gizmos side = new RcVec3f(0, 0, 1); } - RcVec3f forward = new RcVec3f(); - RcVec3f.Cross(ref forward, side, axis); - RcVec3f.Cross(ref side, axis, forward); + var forward = RcVec3f.Cross(side, axis); + side = RcVec3f.Cross(axis, forward); side = RcVec3f.Normalize(side); return side; } diff --git a/src/DotRecast.Recast/RcCommons.cs b/src/DotRecast.Recast/RcCommons.cs index ac48d4d..3ccc044 100644 --- a/src/DotRecast.Recast/RcCommons.cs +++ b/src/DotRecast.Recast/RcCommons.cs @@ -142,7 +142,7 @@ namespace DotRecast.Recast RcVec3f e1 = new RcVec3f(); RcVec3f.Sub(ref e0, verts, v1 * 3, v0 * 3); RcVec3f.Sub(ref e1, verts, v2 * 3, v0 * 3); - RcVec3f.Cross(ref norm, e0, e1); + norm = RcVec3f.Cross(e0, e1); norm = RcVec3f.Normalize(norm); } diff --git a/src/DotRecast.Recast/RcFilledVolumeRasterization.cs b/src/DotRecast.Recast/RcFilledVolumeRasterization.cs index 4282986..2f71c61 100644 --- a/src/DotRecast.Recast/RcFilledVolumeRasterization.cs +++ b/src/DotRecast.Recast/RcFilledVolumeRasterization.cs @@ -176,7 +176,7 @@ namespace DotRecast.Recast private static void Plane(float[][] planes, int p, float[] v1, float[] v2, float[] vertices, int vert) { - RcVec3f.Cross(planes[p], v1, v2); + RcVecUtils.Cross(planes[p], v1, v2); planes[p][3] = planes[p][0] * vertices[vert] + planes[p][1] * vertices[vert + 1] + planes[p][2] * vertices[vert + 2]; } diff --git a/test/DotRecast.Core.Test/Vector3Tests.cs b/test/DotRecast.Core.Test/Vector3Tests.cs index 5c35582..dd6a7f0 100644 --- a/test/DotRecast.Core.Test/Vector3Tests.cs +++ b/test/DotRecast.Core.Test/Vector3Tests.cs @@ -75,4 +75,21 @@ public class Vector3Tests Assert.That(v2.Y, Is.EqualTo(v22.Y).Within(0.000001d)); Assert.That(v2.Z, Is.EqualTo(v22.Z).Within(0.000001d)); } + + [Test] + [Repeat(10000)] + public void TestVectorCross() + { + var v1 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle()); + var v2 = new Vector3(Random.Shared.NextSingle(), Random.Shared.NextSingle(), Random.Shared.NextSingle()); + var v3 = Vector3.Cross(v1, v2); + + var v11 = new RcVec3f(v1.X, v1.Y, v1.Z); + var v22 = new RcVec3f(v2.X, v2.Y, v2.Z); + var v33 = RcVec3f.Cross(v11, v22); + + Assert.That(v3.X, Is.EqualTo(v33.X)); + Assert.That(v3.Y, Is.EqualTo(v33.Y)); + Assert.That(v3.Z, Is.EqualTo(v33.Z)); + } } \ No newline at end of file