diff --git a/src/DotRecast.Core/RecastMath.cs b/src/DotRecast.Core/RecastMath.cs index 6be148a..638006f 100644 --- a/src/DotRecast.Core/RecastMath.cs +++ b/src/DotRecast.Core/RecastMath.cs @@ -116,15 +116,6 @@ namespace DotRecast.Core /// @param[in] v1 The base vector. [(x, y, z)] /// @param[in] v2 The vector to scale and add to @p v1. [(x, y, z)] /// @param[in] s The amount to scale @p v2 by before adding to @p v1. - public static float[] vMad(float[] v1, float[] v2, float s) - { - Vector3f dest = new Vector3f(); - dest[0] = v1[0] + v2[0] * s; - dest[1] = v1[1] + v2[1] * s; - dest[2] = v1[2] + v2[2] * s; - return dest.ToArray(); - } - public static Vector3f vMad(Vector3f v1, Vector3f v2, float s) { Vector3f dest = new Vector3f(); @@ -160,15 +151,6 @@ namespace DotRecast.Core return dest.ToArray(); } - public static float[] vLerp(float[] v1, float[] v2, float t) - { - Vector3f dest = new Vector3f(); - dest[0] = v1[0] + (v2[0] - v1[0]) * t; - dest[1] = v1[1] + (v2[1] - v1[1]) * t; - dest[2] = v1[2] + (v2[2] - v1[2]) * t; - return dest.ToArray(); - } - public static Vector3f vLerp(Vector3f v1, Vector3f v2, float t) { Vector3f dest = new Vector3f(); @@ -225,17 +207,6 @@ namespace DotRecast.Core return dest; } - - - public static float[] vAdd(float[] v1, float[] v2) - { - Vector3f dest = new Vector3f(); - dest[0] = v1[0] + v2[0]; - dest[1] = v1[1] + v2[1]; - dest[2] = v1[2] + v2[2]; - return dest.ToArray(); - } - public static Vector3f vAdd(Vector3f v1, Vector3f v2) { Vector3f dest = new Vector3f(); @@ -245,23 +216,6 @@ namespace DotRecast.Core return dest; } - - public static float[] vCopy(float[] @in) - { - float[] @out = new float[3]; - @out[0] = @in[0]; - @out[1] = @in[1]; - @out[2] = @in[2]; - return @out; - } - - public static void vSet(float[] @out, float a, float b, float c) - { - @out[0] = a; - @out[1] = b; - @out[2] = c; - } - public static void vSet(ref Vector3f @out, float a, float b, float c) { @out.x = a; @@ -269,14 +223,6 @@ namespace DotRecast.Core @out.z = c; } - - public static void vCopy(float[] @out, float[] @in) - { - @out[0] = @in[0]; - @out[1] = @in[1]; - @out[2] = @in[2]; - } - public static void vCopy(float[] @out, Vector3f @in) { @out[0] = @in[0]; diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index 992bdab..635e379 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/src/DotRecast.Detour/NavMeshQuery.cs @@ -2068,8 +2068,13 @@ namespace DotRecast.Detour if (fromTile.links[i].refs == to) { int v = fromTile.links[i].edge; - Array.Copy(fromTile.data.verts, fromPoly.verts[v] * 3, left.ToArray(), 0, 3); - Array.Copy(fromTile.data.verts, fromPoly.verts[v] * 3, right.ToArray(), 0, 3); + left[0] = fromTile.data.verts[fromPoly.verts[v] * 3]; + left[1] = fromTile.data.verts[fromPoly.verts[v] * 3 + 1]; + left[2] = fromTile.data.verts[fromPoly.verts[v] * 3 + 2]; + + right[0] = fromTile.data.verts[fromPoly.verts[v] * 3]; + right[1] = fromTile.data.verts[fromPoly.verts[v] * 3 + 1]; + right[2] = fromTile.data.verts[fromPoly.verts[v] * 3 + 2]; return Results.success(new PortalResult(left, right, fromType, toType)); } } @@ -2084,8 +2089,14 @@ namespace DotRecast.Detour if (toTile.links[i].refs == from) { int v = toTile.links[i].edge; - Array.Copy(toTile.data.verts, toPoly.verts[v] * 3, left.ToArray(), 0, 3); - Array.Copy(toTile.data.verts, toPoly.verts[v] * 3, right.ToArray(), 0, 3); + left[0] = toTile.data.verts[toPoly.verts[v] * 3]; + left[1] = toTile.data.verts[toPoly.verts[v] * 3 + 1]; + left[2] = toTile.data.verts[toPoly.verts[v] * 3 + 2]; + + right[0] = toTile.data.verts[toPoly.verts[v] * 3]; + right[1] = toTile.data.verts[toPoly.verts[v] * 3 + 1]; + right[2] = toTile.data.verts[toPoly.verts[v] * 3 + 2]; + return Results.success(new PortalResult(left, right, fromType, toType)); } } @@ -2096,8 +2107,13 @@ namespace DotRecast.Detour // Find portal vertices. int v0 = fromPoly.verts[link.edge]; int v1 = fromPoly.verts[(link.edge + 1) % fromPoly.vertCount]; - Array.Copy(fromTile.data.verts, v0 * 3, left.ToArray(), 0, 3); - Array.Copy(fromTile.data.verts, v1 * 3, right.ToArray(), 0, 3); + left[0] = fromTile.data.verts[v0 * 3]; + left[1] = fromTile.data.verts[v0 * 3 + 1]; + left[2] = fromTile.data.verts[v0 * 3 + 2]; + + right[0] = fromTile.data.verts[v1 * 3]; + right[1] = fromTile.data.verts[v1 * 3 + 1]; + right[2] = fromTile.data.verts[v1 * 3 + 2]; // If the link is at tile boundary, dtClamp the vertices to // the link width. diff --git a/src/DotRecast.Recast/RecastMeshDetail.cs b/src/DotRecast.Recast/RecastMeshDetail.cs index 9d7e813..6735dab 100644 --- a/src/DotRecast.Recast/RecastMeshDetail.cs +++ b/src/DotRecast.Recast/RecastMeshDetail.cs @@ -27,7 +27,6 @@ using static DotRecast.Recast.RecastConstants; namespace DotRecast.Recast { - public class RecastMeshDetail { public const int MAX_VERTS = 127; @@ -51,7 +50,7 @@ namespace DotRecast.Recast { return a[0] * b[0] + a[2] * b[2]; } - + private static float vdot2(Vector3f a, Vector3f b) { return a[0] * b[0] + a[2] * b[2]; @@ -64,7 +63,7 @@ namespace DotRecast.Recast float dy = verts[q + 2] - verts[p + 2]; return dx * dx + dy * dy; } - + private static float vdist2(float[] verts, int p, int q) { return (float)Math.Sqrt(vdistSq2(verts, p, q)); @@ -76,7 +75,7 @@ namespace DotRecast.Recast float dy = q[2] - p[2]; return dx * dx + dy * dy; } - + private static float vdistSq2(float[] p, Vector3f q) { float dx = q[0] - p[0]; @@ -84,7 +83,7 @@ namespace DotRecast.Recast return dx * dx + dy * dy; } - + private static float vdistSq2(Vector3f p, Vector3f q) { float dx = q[0] - p[0]; @@ -97,26 +96,25 @@ namespace DotRecast.Recast { return (float)Math.Sqrt(vdistSq2(p, q)); } - + private static float vdist2(Vector3f p, Vector3f q) { return (float)Math.Sqrt(vdistSq2(p, q)); } - + private static float vdist2(float[] p, Vector3f q) { return (float)Math.Sqrt(vdistSq2(p, q)); } - private static float vdistSq2(float[] p, float[] verts, int q) { float dx = verts[q + 0] - p[0]; float dy = verts[q + 2] - p[2]; return dx * dx + dy * dy; } - + private static float vdistSq2(Vector3f p, float[] verts, int q) { float dx = verts[q + 0] - p[0]; @@ -129,7 +127,7 @@ namespace DotRecast.Recast { return (float)Math.Sqrt(vdistSq2(p, verts, q)); } - + private static float vdist2(Vector3f p, float[] verts, int q) { return (float)Math.Sqrt(vdistSq2(p, verts, q)); @@ -153,7 +151,7 @@ namespace DotRecast.Recast float v2 = p3[2] - p1[2]; return u1 * v2 - v1 * u2; } - + private static float vcross2(Vector3f p1, Vector3f p2, Vector3f p3) { float u1 = p2[0] - p1[0]; @@ -193,7 +191,7 @@ namespace DotRecast.Recast return false; } - private static float distPtTri(float[] p, float[] verts, int a, int b, int c) + private static float distPtTri(Vector3f p, float[] verts, int a, int b, int c) { Vector3f v0 = new Vector3f(); Vector3f v1 = new Vector3f(); @@ -255,6 +253,34 @@ namespace DotRecast.Recast return dx * dx + dy * dy + dz * dz; } + private static float distancePtSeg2d(Vector3f verts, float[] poly, int p, int q) + { + float pqx = poly[q + 0] - poly[p + 0]; + float pqz = poly[q + 2] - poly[p + 2]; + float dx = verts[0] - poly[p + 0]; + float dz = verts[2] - poly[p + 2]; + float d = pqx * pqx + pqz * pqz; + float t = pqx * dx + pqz * dz; + if (d > 0) + { + t /= d; + } + + if (t < 0) + { + t = 0; + } + else if (t > 1) + { + t = 1; + } + + dx = poly[p + 0] + t * pqx - verts[0]; + dz = poly[p + 2] + t * pqz - verts[2]; + + return dx * dx + dz * dz; + } + private static float distancePtSeg2d(float[] verts, int pt, float[] poly, int p, int q) { float pqx = poly[q + 0] - poly[p + 0]; @@ -283,7 +309,7 @@ namespace DotRecast.Recast return dx * dx + dz * dz; } - private static float distToTriMesh(float[] p, float[] verts, int nverts, List tris, int ntris) + private static float distToTriMesh(Vector3f p, float[] verts, int nverts, List tris, int ntris) { float dmin = float.MaxValue; for (int i = 0; i < ntris; ++i) @@ -306,7 +332,7 @@ namespace DotRecast.Recast return dmin; } - private static float distToPoly(int nvert, float[] verts, float[] p) + private static float distToPoly(int nvert, float[] verts, Vector3f p) { float dmin = float.MaxValue; int i, j; @@ -321,7 +347,7 @@ namespace DotRecast.Recast c = !c; } - dmin = Math.Min(dmin, distancePtSeg2d(p, 0, verts, vj, vi)); + dmin = Math.Min(dmin, distancePtSeg2d(p, verts, vj, vi)); } return c ? -dmin : dmin; @@ -1009,7 +1035,7 @@ namespace DotRecast.Recast pt[1] = (bmax[1] + bmin[1]) * 0.5f; pt[2] = z * sampleDist; // Make sure the samples are not too close to the edges. - if (distToPoly(nin, @in, pt.ToArray()) > -sampleDist / 2) + if (distToPoly(nin, @in, pt) > -sampleDist / 2) { continue; } @@ -1050,7 +1076,7 @@ namespace DotRecast.Recast pt[0] = samples[s + 0] * sampleDist + getJitterX(i) * cs * 0.1f; pt[1] = samples[s + 1] * chf.ch; pt[2] = samples[s + 2] * sampleDist + getJitterY(i) * cs * 0.1f; - float d = distToTriMesh(pt.ToArray(), verts, nverts, tris, tris.Count / 4); + float d = distToTriMesh(pt, verts, nverts, tris, tris.Count / 4); if (d < 0) { continue; // did not hit the mesh. diff --git a/src/DotRecast.Recast/RecastVectors.cs b/src/DotRecast.Recast/RecastVectors.cs index bd29e5e..a8e793c 100644 --- a/src/DotRecast.Recast/RecastVectors.cs +++ b/src/DotRecast.Recast/RecastVectors.cs @@ -177,7 +177,7 @@ namespace DotRecast.Recast e0[2] = i[2] - verts[j + 2]; } - public static void sub(ref Vector3f e0, float[] i, float[] verts, int j) + public static void sub(ref Vector3f e0, Vector3f i, float[] verts, int j) { e0[0] = i[0] - verts[j]; e0[1] = i[1] - verts[j + 1]; diff --git a/test/DotRecast.Detour.Test/FindLocalNeighbourhoodTest.cs b/test/DotRecast.Detour.Test/FindLocalNeighbourhoodTest.cs index 02e5d94..b7dd9bb 100644 --- a/test/DotRecast.Detour.Test/FindLocalNeighbourhoodTest.cs +++ b/test/DotRecast.Detour.Test/FindLocalNeighbourhoodTest.cs @@ -56,8 +56,7 @@ public class FindLocalNeighbourhoodTest : AbstractDetourTest for (int i = 0; i < startRefs.Length; i++) { Vector3f startPos = startPoss[i]; - Result poly = query.findLocalNeighbourhood(startRefs[i], startPos, 3.5f, - filter); + Result poly = query.findLocalNeighbourhood(startRefs[i], startPos, 3.5f, filter); Assert.That(poly.result.getRefs().Count, Is.EqualTo(REFS[i].Length)); for (int v = 0; v < REFS[i].Length; v++) {