diff --git a/src/DotRecast.Core/RcMath.cs b/src/DotRecast.Core/RcMath.cs index 398848a..f7830f2 100644 --- a/src/DotRecast.Core/RcMath.cs +++ b/src/DotRecast.Core/RcMath.cs @@ -142,7 +142,14 @@ namespace DotRecast.Core return overlap; } - public static Tuple DistancePtSegSqr2D(Vector3f pt, Vector3f p, Vector3f q) + public static DistSeg DistancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q) + { + var vp = Vector3f.Of(verts, p); + var vq = Vector3f.Of(verts, q); + return DistancePtSegSqr2D(pt, vp, vq); + } + + public static DistSeg DistancePtSegSqr2D(Vector3f pt, Vector3f p, Vector3f q) { float pqx = q.x - p.x; float pqz = q.z - p.z; @@ -166,7 +173,11 @@ namespace DotRecast.Core dx = p.x + t * pqx - pt.x; dz = p.z + t * pqz - pt.z; - return Tuple.Create(dx * dx + dz * dz, t); + return new DistSeg() + { + DistSqr = dx * dx + dz * dz, + Seg = t, + }; } public static float? ClosestHeightPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c) @@ -454,66 +465,6 @@ namespace DotRecast.Core 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.x - verts[p + 0]; - float dz = pt.z - 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.x; - dz = verts[p + 2] + t * pqz - pt.z; - return Tuple.Create(dx * dx + dz * dz, t); - } - - - public static DistSeg DistancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q) - { - float pqx = verts[q + 0] - verts[p + 0]; - float pqz = verts[q + 2] - verts[p + 2]; - float dx = pt.x - verts[p + 0]; - float dz = pt.z - 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.x; - dz = verts[p + 2] + t * pqz - pt.z; - - return new DistSeg() - { - DistSqr = dx * dx + dz * dz, - Seg = t, - }; - } - public static int OppositeTile(int side) { return (side + 4) & 0x7; diff --git a/src/DotRecast.Detour.Crowd/LocalBoundary.cs b/src/DotRecast.Detour.Crowd/LocalBoundary.cs index 4915672..8e5996d 100644 --- a/src/DotRecast.Detour.Crowd/LocalBoundary.cs +++ b/src/DotRecast.Detour.Crowd/LocalBoundary.cs @@ -116,14 +116,17 @@ namespace DotRecast.Detour.Crowd for (int k = 0; k < gpws.CountSegmentRefs(); ++k) { SegmentVert s = gpws.GetSegmentVert(k); + var s0 = Vector3f.Of(s[0], s[1], s[2]); + var s3 = Vector3f.Of(s[3], s[4], s[5]); + // Skip too distant segments. - Tuple distseg = DistancePtSegSqr2D(pos, s, 0, 3); - if (distseg.Item1 > Sqr(collisionQueryRange)) + var distseg = DistancePtSegSqr2D(pos, s0, s3); + if (distseg.DistSqr > Sqr(collisionQueryRange)) { continue; } - AddSegment(distseg.Item1, s); + AddSegment(distseg.DistSqr, s); } } } diff --git a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs index 017a4a3..799e52c 100644 --- a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs +++ b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs @@ -150,8 +150,8 @@ namespace DotRecast.Detour.Crowd // Precalc if the agent is really close to the segment. float r = 0.01f; - Tuple dt = DistancePtSegSqr2D(pos, seg.p, seg.q); - seg.touch = dt.Item1 < Sqr(r); + var dt = DistancePtSegSqr2D(pos, seg.p, seg.q); + seg.touch = dt.DistSqr < Sqr(r); } } diff --git a/src/DotRecast.Detour/NavMesh.cs b/src/DotRecast.Detour/NavMesh.cs index 12120ec..49b472c 100644 --- a/src/DotRecast.Detour/NavMesh.cs +++ b/src/DotRecast.Detour/NavMesh.cs @@ -1187,9 +1187,9 @@ namespace DotRecast.Detour continue; } - Tuple dt = DistancePtSegSqr2D(pos, v[j], v[k]); - float d = dt.Item1; - float t = dt.Item2; + var dt = DistancePtSegSqr2D(pos, v[j], v[k]); + float d = dt.DistSqr; + float t = dt.Seg; if (d < dmin) { dmin = d; @@ -1213,9 +1213,9 @@ namespace DotRecast.Detour v[1].y = tile.data.verts[poly.verts[k] * 3 + 1]; v[1].z = tile.data.verts[poly.verts[k] * 3 + 2]; - Tuple dt = DistancePtSegSqr2D(pos, v[0], v[1]); - float d = dt.Item1; - float t = dt.Item2; + var dt = DistancePtSegSqr2D(pos, v[0], v[1]); + float d = dt.DistSqr; + float t = dt.Seg; if (d < dmin) { dmin = d; @@ -1343,8 +1343,8 @@ namespace DotRecast.Detour var v0 = new Vector3f { x = tile.data.verts[i], y = tile.data.verts[i + 1], z = tile.data.verts[i + 2] }; i = poly.verts[1] * 3; var v1 = new Vector3f { x = tile.data.verts[i], y = tile.data.verts[i + 1], z = tile.data.verts[i + 2] }; - Tuple dt = DistancePtSegSqr2D(pos, v0, v1); - return new ClosestPointOnPolyResult(false, Vector3f.Lerp(v0, v1, dt.Item2)); + var dt = DistancePtSegSqr2D(pos, v0, v1); + return new ClosestPointOnPolyResult(false, Vector3f.Lerp(v0, v1, dt.Seg)); } // Outside poly that is not an offmesh connection. diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index e63a319..247d7cd 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/src/DotRecast.Detour/NavMeshQuery.cs @@ -347,7 +347,7 @@ namespace DotRecast.Detour // If the circle is not touching the next polygon, skip it. var distseg = DistancePtSegSqr2D(centerPos, va, vb); - float distSqr = distseg.Item1; + float distSqr = distseg.DistSqr; if (distSqr > radiusSqr) { continue; @@ -541,7 +541,7 @@ namespace DotRecast.Detour i = poly.verts[1] * 3; var v1 = new Vector3f { x = tile.data.verts[i], y = tile.data.verts[i + 1], z = tile.data.verts[i + 2] }; var dt = DistancePtSegSqr2D(pos, v0, v1); - return Results.Success(v0.y + (v1.y - v0.y) * dt.Item2); + return Results.Success(v0.y + (v1.y - v0.y) * dt.Seg); } float? height = m_nav.GetPolyHeight(tile, poly, pos); @@ -1626,8 +1626,8 @@ namespace DotRecast.Detour // If starting really close the portal, advance. if (i == 0) { - Tuple dt = DistancePtSegSqr2D(portalApex, left, right); - if (dt.Item1 < Sqr(0.001f)) + var dt = DistancePtSegSqr2D(portalApex, left, right); + if (dt.DistSqr < Sqr(0.001f)) { continue; } @@ -2553,8 +2553,8 @@ namespace DotRecast.Detour var vb = pp.result.right; // If the circle is not touching the next polygon, skip it. - Tuple distseg = DistancePtSegSqr2D(centerPos, va, vb); - float distSqr = distseg.Item1; + var distseg = DistancePtSegSqr2D(centerPos, va, vb); + float distSqr = distseg.DistSqr; if (distSqr > radiusSqr) { continue; @@ -2917,8 +2917,8 @@ namespace DotRecast.Detour var vb = pp.result.right; // If the circle is not touching the next polygon, skip it. - Tuple distseg = DistancePtSegSqr2D(centerPos, va, vb); - float distSqr = distseg.Item1; + var distseg = DistancePtSegSqr2D(centerPos, va, vb); + float distSqr = distseg.DistSqr; if (distSqr > radiusSqr) { continue; diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index 94700bd..94ffa57 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -847,10 +847,11 @@ public class TestNavmeshTool : Tool for (int j = 0; j < wallSegments.CountSegmentVerts(); ++j) { SegmentVert s = wallSegments.GetSegmentVert(j); - Vector3f s3 = Vector3f.Of(s[3], s[4], s[5]); + var v0 = Vector3f.Of(s[0], s[1], s[2]); + var s3 = Vector3f.Of(s[3], s[4], s[5]); // Skip too distant segments. - Tuple distSqr = DistancePtSegSqr2D(m_spos, s, 0, 3); - if (distSqr.Item1 > RcMath.Sqr(m_neighbourhoodRadius)) + var distSqr = DistancePtSegSqr2D(m_spos, v0, s3); + if (distSqr.DistSqr > RcMath.Sqr(m_neighbourhoodRadius)) { continue; } @@ -1016,4 +1017,4 @@ public class TestNavmeshTool : Tool } } } -} +} \ No newline at end of file