diff --git a/src/DotRecast.Core/RcMath.cs b/src/DotRecast.Core/RcMath.cs index e9a2aa8..3aeb85d 100644 --- a/src/DotRecast.Core/RcMath.cs +++ b/src/DotRecast.Core/RcMath.cs @@ -27,32 +27,6 @@ namespace DotRecast.Core public const float EPS = 1e-4f; private static readonly float EQUAL_THRESHOLD = Sqr(1.0f / 16384.0f); - public static float VDistSqr(Vector3f v1, float[] v2, int i) - { - float dx = v2[i] - v1.x; - float dy = v2[i + 1] - v1.y; - float dz = v2[i + 2] - v1.z; - return dx * dx + dy * dy + dz * dz; - } - - public static float VDistSqr(Vector3f v1, Vector3f v2) - { - float dx = v2.x - v1.x; - float dy = v2.y - v1.y; - float dz = v2.z - v1.z; - return dx * dx + dy * dy + dz * dz; - } - - public static float VDistSqr(float[] v, int i, int j) - { - float dx = v[i] - v[j]; - float dy = v[i + 1] - v[j + 1]; - float dz = v[i + 2] - v[j + 2]; - return dx * dx + dy * dy + dz * dz; - } - - - public static float Sqr(float f) { return f * f; @@ -91,37 +65,29 @@ namespace DotRecast.Core } - - /// Performs a linear interpolation between two vectors. (@p v1 toward @p - /// v2) - /// @param[out] dest The result vector. [(x, y, x)] - /// @param[in] v1 The starting vector. - /// @param[in] v2 The destination vector. - /// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0] - public static Vector3f VLerp(float[] verts, int v1, int v2, float t) + public static float VDistSqr(Vector3f v1, float[] v2, int i) { - return new Vector3f( - verts[v1 + 0] + (verts[v2 + 0] - verts[v1 + 0]) * t, - verts[v1 + 1] + (verts[v2 + 1] - verts[v1 + 1]) * t, - verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t - ); - } - - public static void VMin(ref Vector3f @out, float[] @in, int i) - { - @out.x = Math.Min(@out.x, @in[i]); - @out.y = Math.Min(@out.y, @in[i + 1]); - @out.z = Math.Min(@out.z, @in[i + 2]); + float dx = v2[i] - v1.x; + float dy = v2[i + 1] - v1.y; + float dz = v2[i + 2] - v1.z; + return dx * dx + dy * dy + dz * dz; } - - public static void VMax(ref Vector3f @out, float[] @in, int i) + public static float VDistSqr(Vector3f v1, Vector3f v2) { - @out.x = Math.Max(@out.x, @in[i]); - @out.y = Math.Max(@out.y, @in[i + 1]); - @out.z = Math.Max(@out.z, @in[i + 2]); + float dx = v2.x - v1.x; + float dy = v2.y - v1.y; + float dz = v2.z - v1.z; + return dx * dx + dy * dy + dz * dz; } + public static float VDistSqr(float[] v, int i, int j) + { + float dx = v[i] - v[j]; + float dy = v[i + 1] - v[j + 1]; + float dz = v[i + 2] - v[j + 2]; + return dx * dx + dy * dy + dz * dz; + } /// Returns the distance between two points. /// @param[in] v1 A point. [(x, y, z)] diff --git a/src/DotRecast.Core/Vector3f.cs b/src/DotRecast.Core/Vector3f.cs index 59c307d..2b2bdf2 100644 --- a/src/DotRecast.Core/Vector3f.cs +++ b/src/DotRecast.Core/Vector3f.cs @@ -217,6 +217,22 @@ namespace DotRecast.Core z *= d; } } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Min(float[] @in, int i) + { + x = Math.Min(x, @in[i]); + y = Math.Min(y, @in[i + 1]); + z = Math.Min(z, @in[i + 2]); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Max(float[] @in, int i) + { + x = Math.Max(x, @in[i]); + y = Math.Max(y, @in[i + 1]); + z = Math.Max(z, @in[i + 2]); + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool operator ==(Vector3f left, Vector3f right) @@ -325,5 +341,21 @@ namespace DotRecast.Core z = v1.z + (v2.z * s), }; } + + /// Performs a linear interpolation between two vectors. (@p v1 toward @p + /// v2) + /// @param[out] dest The result vector. [(x, y, x)] + /// @param[in] v1 The starting vector. + /// @param[in] v2 The destination vector. + /// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3f Lerp(float[] verts, int v1, int v2, float t) + { + return new Vector3f( + verts[v1 + 0] + (verts[v2 + 0] - verts[v1 + 0]) * t, + verts[v1 + 1] + (verts[v2 + 1] - verts[v1 + 1]) * t, + verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t + ); + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Extras/BVTreeBuilder.cs b/src/DotRecast.Detour.Extras/BVTreeBuilder.cs index 9e1172f..22e1b52 100644 --- a/src/DotRecast.Detour.Extras/BVTreeBuilder.cs +++ b/src/DotRecast.Detour.Extras/BVTreeBuilder.cs @@ -45,8 +45,8 @@ namespace DotRecast.Detour.Extras bmax.Set(data.verts, data.polys[i].verts[0] * 3); for (int j = 1; j < data.polys[i].vertCount; j++) { - VMin(ref bmin, data.verts, data.polys[i].verts[j] * 3); - VMax(ref bmax, data.verts, data.polys[i].verts[j] * 3); + bmin.Min(data.verts, data.polys[i].verts[j] * 3); + bmax.Max(data.verts, data.polys[i].verts[j] * 3); } it.bmin[0] = Clamp((int)((bmin.x - data.header.bmin.x) * quantFactor), 0, 0x7fffffff); diff --git a/src/DotRecast.Detour/NavMesh.cs b/src/DotRecast.Detour/NavMesh.cs index c36c1a9..1fef36d 100644 --- a/src/DotRecast.Detour/NavMesh.cs +++ b/src/DotRecast.Detour/NavMesh.cs @@ -412,8 +412,8 @@ namespace DotRecast.Detour for (int j = 1; j < p.vertCount; ++j) { v = p.verts[j] * 3; - VMin(ref bmin, tile.data.verts, v); - VMax(ref bmax, tile.data.verts, v); + bmin.Min(tile.data.verts, v); + bmax.Max(tile.data.verts, v); } if (OverlapBounds(qmin, qmax, bmin, bmax)) diff --git a/src/DotRecast.Detour/NavMeshBuilder.cs b/src/DotRecast.Detour/NavMeshBuilder.cs index 5eeb219..7ef1a98 100644 --- a/src/DotRecast.Detour/NavMeshBuilder.cs +++ b/src/DotRecast.Detour/NavMeshBuilder.cs @@ -166,8 +166,8 @@ namespace DotRecast.Detour bmax.Set(option.detailVerts, dv); for (int j = 1; j < ndv; j++) { - VMin(ref bmin, option.detailVerts, dv + j * 3); - VMax(ref bmax, option.detailVerts, dv + j * 3); + bmin.Min(option.detailVerts, dv + j * 3); + bmax.Max(option.detailVerts, dv + j * 3); } // BV-tree uses cs for all dimensions diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index 5b167eb..3dec3db 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/src/DotRecast.Detour/NavMeshQuery.cs @@ -499,7 +499,7 @@ namespace DotRecast.Detour int va = imin * 3; int vb = ((imin + 1) % nv) * 3; - closest = VLerp(verts, va, vb, edget[imin]); + closest = Vector3f.Lerp(verts, va, vb, edget[imin]); } return Results.Success(closest); @@ -656,8 +656,8 @@ namespace DotRecast.Detour for (int j = 1; j < p.vertCount; ++j) { v = p.verts[j] * 3; - VMin(ref bmin, tile.data.verts, v); - VMax(ref bmax, tile.data.verts, v); + bmin.Min(tile.data.verts, v); + bmax.Max(tile.data.verts, v); } if (OverlapBounds(qmin, qmax, bmin, bmax)) @@ -1912,7 +1912,7 @@ namespace DotRecast.Detour if (distSqr < bestDist) { // Update nearest distance. - bestPos = VLerp(verts, vj, vi, tseg); + bestPos = Vector3f.Lerp(verts, vj, vi, tseg); bestDist = distSqr; bestNode = curNode; } @@ -2089,8 +2089,8 @@ namespace DotRecast.Detour float s = 1.0f / 255.0f; float tmin = link.bmin * s; float tmax = link.bmax * s; - left = VLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin); - right = VLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax); + left = Vector3f.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin); + right = Vector3f.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax); } } @@ -3123,8 +3123,8 @@ namespace DotRecast.Detour float tmin = ints[k].tmin / 255.0f; float tmax = ints[k].tmax / 255.0f; var seg = new SegmentVert(); - seg.vmin = VLerp(tile.data.verts, vj, vi, tmin); - seg.vmax = VLerp(tile.data.verts, vj, vi, tmax); + seg.vmin = Vector3f.Lerp(tile.data.verts, vj, vi, tmin); + seg.vmax = Vector3f.Lerp(tile.data.verts, vj, vi, tmax); segmentVerts.Add(seg); segmentRefs.Add(ints[k].refs); } @@ -3137,8 +3137,8 @@ namespace DotRecast.Detour float tmin = imin / 255.0f; float tmax = imax / 255.0f; var seg = new SegmentVert(); - seg.vmin = VLerp(tile.data.verts, vj, vi, tmin); - seg.vmax = VLerp(tile.data.verts, vj, vi, tmax); + seg.vmin = Vector3f.Lerp(tile.data.verts, vj, vi, tmin); + seg.vmax = Vector3f.Lerp(tile.data.verts, vj, vi, tmax); segmentVerts.Add(seg); segmentRefs.Add(0L); }