diff --git a/src/DotRecast.Core/Numerics/RcVec3f.cs b/src/DotRecast.Core/Numerics/RcVec3f.cs index 07aac41..a78cd59 100644 --- a/src/DotRecast.Core/Numerics/RcVec3f.cs +++ b/src/DotRecast.Core/Numerics/RcVec3f.cs @@ -469,32 +469,6 @@ namespace DotRecast.Core.Numerics @out[n + 2] = @in[m + 2]; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Add(ref RcVec3f e0, RcVec3f a, float[] verts, int i) - { - e0.X = a.X + verts[i]; - e0.Y = a.Y + verts[i + 1]; - e0.Z = a.Z + verts[i + 2]; - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Sub(ref RcVec3f e0, float[] verts, int i, int j) - { - e0.X = verts[i] - verts[j]; - e0.Y = verts[i + 1] - verts[j + 1]; - e0.Z = verts[i + 2] - verts[j + 2]; - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Sub(ref RcVec3f e0, RcVec3f i, float[] verts, int j) - { - e0.X = i.X - verts[j]; - e0.Y = i.Y - verts[j + 1]; - e0.Z = i.Z - verts[j + 2]; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static RcVec3f Cross(RcVec3f v1, RcVec3f v2) { diff --git a/src/DotRecast.Core/Numerics/RcVecUtils.cs b/src/DotRecast.Core/Numerics/RcVecUtils.cs index 7f12d90..3899f81 100644 --- a/src/DotRecast.Core/Numerics/RcVecUtils.cs +++ b/src/DotRecast.Core/Numerics/RcVecUtils.cs @@ -55,7 +55,40 @@ namespace DotRecast.Core.Numerics return @this.X * v[vi] + @this.Z * v[vi + 2]; } - + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static RcVec3f Add(RcVec3f a, float[] verts, int i) + { + return new RcVec3f( + a.X + verts[i], + a.Y + verts[i + 1], + a.Z + verts[i + 2] + ); + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static RcVec3f Subtract(float[] verts, int i, int j) + { + return new RcVec3f( + verts[i] - verts[j], + verts[i + 1] - verts[j + 1], + verts[i + 2] - verts[j + 2] + ); + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static RcVec3f Subtract(RcVec3f i, float[] verts, int j) + { + return new RcVec3f( + i.X - verts[j], + i.Y - verts[j + 1], + i.Z - verts[j + 2] + ); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Cross(float[] dest, float[] v1, float[] v2) { diff --git a/src/DotRecast.Recast/RcCommons.cs b/src/DotRecast.Recast/RcCommons.cs index 3ccc044..2b41634 100644 --- a/src/DotRecast.Recast/RcCommons.cs +++ b/src/DotRecast.Recast/RcCommons.cs @@ -138,10 +138,8 @@ namespace DotRecast.Recast public static void CalcTriNormal(float[] verts, int v0, int v1, int v2, ref RcVec3f norm) { - RcVec3f e0 = new RcVec3f(); - RcVec3f e1 = new RcVec3f(); - RcVec3f.Sub(ref e0, verts, v1 * 3, v0 * 3); - RcVec3f.Sub(ref e1, verts, v2 * 3, v0 * 3); + var e0 = RcVecUtils.Subtract(verts, v1 * 3, v0 * 3); + var e1 = RcVecUtils.Subtract(verts, v2 * 3, v0 * 3); norm = RcVec3f.Cross(e0, e1); norm = RcVec3f.Normalize(norm); } diff --git a/src/DotRecast.Recast/RcMeshDetails.cs b/src/DotRecast.Recast/RcMeshDetails.cs index d77de8d..fa3dc4e 100644 --- a/src/DotRecast.Recast/RcMeshDetails.cs +++ b/src/DotRecast.Recast/RcMeshDetails.cs @@ -160,11 +160,9 @@ namespace DotRecast.Recast { const float EPS = 1e-6f; // Calculate the circle relative to p1, to avoid some precision issues. - RcVec3f v1 = new RcVec3f(); - RcVec3f v2 = new RcVec3f(); - RcVec3f v3 = new RcVec3f(); - RcVec3f.Sub(ref v2, verts, p2, p1); - RcVec3f.Sub(ref v3, verts, p3, p1); + var v1 = new RcVec3f(); + var v2 = RcVecUtils.Subtract(verts, p2, p1); + var v3 = RcVecUtils.Subtract(verts, p3, p1); float cp = Vcross2(v1, v2, v3); if (Math.Abs(cp) > EPS) @@ -176,7 +174,7 @@ namespace DotRecast.Recast c.Y = 0; c.Z = (v1Sq * (v3.X - v2.X) + v2Sq * (v1.X - v3.X) + v3Sq * (v2.X - v1.X)) / (2 * cp); r.Exchange(Vdist2(c, v1)); - RcVec3f.Add(ref c, c, verts, p1); + c = RcVecUtils.Add(c, verts, p1); return true; } @@ -187,12 +185,9 @@ namespace DotRecast.Recast private static float DistPtTri(RcVec3f p, float[] verts, int a, int b, int c) { - RcVec3f v0 = new RcVec3f(); - RcVec3f v1 = new RcVec3f(); - RcVec3f v2 = new RcVec3f(); - RcVec3f.Sub(ref v0, verts, c, a); - RcVec3f.Sub(ref v1, verts, b, a); - RcVec3f.Sub(ref v2, p, verts, a); + var v0 = RcVecUtils.Subtract(verts, c, a); + var v1 = RcVecUtils.Subtract(verts, b, a); + var v2 = RcVecUtils.Subtract(p, verts, a); float dot00 = Vdot2(v0, v0); float dot01 = Vdot2(v0, v1);