diff --git a/src/DotRecast.Core/RecastMath.cs b/src/DotRecast.Core/RecastMath.cs index 1c1c5fe..6aeffe1 100644 --- a/src/DotRecast.Core/RecastMath.cs +++ b/src/DotRecast.Core/RecastMath.cs @@ -880,6 +880,34 @@ namespace DotRecast.Core result.intersects = true; return result; } + + public static Tuple distancePtSegSqr2D(Vector3f pt, SegmentVert verts, int p, int q) + { + float pqx = verts[q + 0] - verts[p + 0]; + float pqz = verts[q + 2] - verts[p + 2]; + float dx = pt[0] - verts[p + 0]; + float dz = pt[2] - verts[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 = verts[p + 0] + t * pqx - pt[0]; + dz = verts[p + 2] + t * pqz - pt[2]; + return Tuple.Create(dx * dx + dz * dz, t); + } + public static Tuple distancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q) { diff --git a/src/DotRecast.Core/SegmentVert.cs b/src/DotRecast.Core/SegmentVert.cs new file mode 100644 index 0000000..bdb5c10 --- /dev/null +++ b/src/DotRecast.Core/SegmentVert.cs @@ -0,0 +1,29 @@ +using System; + +namespace DotRecast.Core +{ + public struct SegmentVert + { + public Vector3f vmin; + public Vector3f vmax; + + public float this[int index] + { + get => GetElement(index); + } + + public float GetElement(int index) + { + switch (index) + { + case 0: return vmin.x; + case 1: return vmin.y; + case 2: return vmin.z; + case 3: return vmax.x; + case 4: return vmax.y; + case 5: return vmax.z; + default: throw new IndexOutOfRangeException($"{index}"); + } + } + } +} \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/LocalBoundary.cs b/src/DotRecast.Detour.Crowd/LocalBoundary.cs index d58e37a..c51004b 100644 --- a/src/DotRecast.Detour.Crowd/LocalBoundary.cs +++ b/src/DotRecast.Detour.Crowd/LocalBoundary.cs @@ -55,16 +55,12 @@ namespace DotRecast.Detour.Crowd m_segs.Clear(); } - protected void addSegment(float dist, float[] s) + protected void addSegment(float dist, SegmentVert s) { // Insert neighbour based on the distance. Segment seg = new Segment(); - seg.s[0].x = s[0]; - seg.s[0].y = s[1]; - seg.s[0].z = s[2]; - seg.s[1].x = s[3]; - seg.s[1].y = s[4]; - seg.s[1].z = s[5]; + seg.s[0] = s.vmin; + seg.s[1] = s.vmax; //Array.Copy(s, seg.s, 6); seg.d = dist; if (0 == m_segs.Count) @@ -126,7 +122,7 @@ namespace DotRecast.Detour.Crowd GetPolyWallSegmentsResult gpws = result.result; for (int k = 0; k < gpws.getSegmentRefs().Count; ++k) { - float[] s = gpws.getSegmentVerts()[k]; + SegmentVert s = gpws.getSegmentVerts()[k]; // Skip too distant segments. Tuple distseg = distancePtSegSqr2D(pos, s, 0, 3); if (distseg.Item1 > sqr(collisionQueryRange)) diff --git a/src/DotRecast.Detour/GetPolyWallSegmentsResult.cs b/src/DotRecast.Detour/GetPolyWallSegmentsResult.cs index 86feda4..ba7e49d 100644 --- a/src/DotRecast.Detour/GetPolyWallSegmentsResult.cs +++ b/src/DotRecast.Detour/GetPolyWallSegmentsResult.cs @@ -19,21 +19,22 @@ freely, subject to the following restrictions: */ using System.Collections.Generic; +using DotRecast.Core; namespace DotRecast.Detour { public class GetPolyWallSegmentsResult { - private readonly List segmentVerts; + private readonly List segmentVerts; private readonly List segmentRefs; - public GetPolyWallSegmentsResult(List segmentVerts, List segmentRefs) + public GetPolyWallSegmentsResult(List segmentVerts, List segmentRefs) { this.segmentVerts = segmentVerts; this.segmentRefs = segmentRefs; } - public List getSegmentVerts() + public List getSegmentVerts() { return segmentVerts; } diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index 025d7c7..94e19fc 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/src/DotRecast.Detour/NavMeshQuery.cs @@ -3096,7 +3096,7 @@ namespace DotRecast.Detour Poly poly = tileAndPoly.result.Item2; List segmentRefs = new List(); - List segmentVerts = new List(); + List segmentVerts = new List(); List ints = new List(16); for (int i = 0, j = poly.vertCount - 1; i < poly.vertCount; j = i++) @@ -3146,9 +3146,11 @@ namespace DotRecast.Detour int ivj = poly.verts[j] * 3; int ivi = poly.verts[i] * 3; - float[] seg = new float[6]; - Array.Copy(tile.data.verts, ivj, seg, 0, 3); - Array.Copy(tile.data.verts, ivi, seg, 3, 3); + var seg = new SegmentVert(); + vCopy(ref seg.vmin, tile.data.verts, ivj); + vCopy(ref seg.vmax, tile.data.verts, ivi); + // Array.Copy(tile.data.verts, ivj, seg, 0, 3); + // Array.Copy(tile.data.verts, ivi, seg, 3, 3); segmentVerts.Add(seg); segmentRefs.Add(neiRef); continue; @@ -3168,15 +3170,9 @@ namespace DotRecast.Detour { float tmin = ints[k].tmin / 255.0f; float tmax = ints[k].tmax / 255.0f; - float[] seg = new float[6]; - var vmin = vLerp(tile.data.verts, vj, vi, tmin); - var vmax = vLerp(tile.data.verts, vj, vi, tmax); - seg[0] = vmin[0]; - seg[1] = vmin[1]; - seg[2] = vmin[2]; - seg[3] = vmax[0]; - seg[4] = vmax[1]; - seg[5] = vmax[2]; + var seg = new SegmentVert(); + seg.vmin = vLerp(tile.data.verts, vj, vi, tmin); + seg.vmax = vLerp(tile.data.verts, vj, vi, tmax); segmentVerts.Add(seg); segmentRefs.Add(ints[k].refs); } @@ -3188,15 +3184,9 @@ namespace DotRecast.Detour { float tmin = imin / 255.0f; float tmax = imax / 255.0f; - float[] seg = new float[6]; - var vmin = vLerp(tile.data.verts, vj, vi, tmin); - var vmax = vLerp(tile.data.verts, vj, vi, tmax); - seg[0] = vmin[0]; - seg[1] = vmin[1]; - seg[2] = vmin[2]; - seg[3] = vmax[0]; - seg[4] = vmax[1]; - seg[5] = vmax[2]; + var seg = new SegmentVert(); + seg.vmin = vLerp(tile.data.verts, vj, vi, tmin); + seg.vmax = vLerp(tile.data.verts, vj, vi, tmax); segmentVerts.Add(seg); segmentRefs.Add(0L); } diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index 4bbbcb3..abf32e3 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -845,7 +845,7 @@ public class TestNavmeshTool : Tool GetPolyWallSegmentsResult wallSegments = result.result; for (int j = 0; j < wallSegments.getSegmentVerts().Count; ++j) { - float[] s = wallSegments.getSegmentVerts()[j]; + SegmentVert s = wallSegments.getSegmentVerts()[j]; Vector3f s3 = Vector3f.Of(s[3], s[4], s[5]); // Skip too distant segments. Tuple distSqr = distancePtSegSqr2D(m_spos, s, 0, 3); @@ -854,8 +854,8 @@ public class TestNavmeshTool : Tool continue; } - Vector3f delta = vSub(s3, s); - Vector3f p0 = vMad(s, delta, 0.5f); + Vector3f delta = vSub(s3, s.vmin); + Vector3f p0 = vMad(s.vmin, delta, 0.5f); Vector3f norm = Vector3f.Of(delta[2], 0, -delta[0]); vNormalize(ref norm); Vector3f p1 = vMad(p0, norm, agentRadius * 0.5f); @@ -869,7 +869,7 @@ public class TestNavmeshTool : Tool else { int col = duRGBA(192, 32, 16, 192); - if (triArea2D(m_spos, s, s3) < 0.0f) + if (triArea2D(m_spos, s.vmin, s3) < 0.0f) { col = duRGBA(96, 32, 16, 192); }