diff --git a/src/DotRecast.Core/DistSeg.cs b/src/DotRecast.Core/DistSeg.cs deleted file mode 100644 index bd27968..0000000 --- a/src/DotRecast.Core/DistSeg.cs +++ /dev/null @@ -1,8 +0,0 @@ -namespace DotRecast.Core -{ - public struct DistSeg - { - public float DistSqr; - public float Seg; - } -} \ No newline at end of file diff --git a/src/DotRecast.Core/RcMath.cs b/src/DotRecast.Core/RcMath.cs index f7830f2..446b115 100644 --- a/src/DotRecast.Core/RcMath.cs +++ b/src/DotRecast.Core/RcMath.cs @@ -142,21 +142,21 @@ namespace DotRecast.Core return overlap; } - public static DistSeg DistancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q) + public static float DistancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q, out float t) { var vp = Vector3f.Of(verts, p); var vq = Vector3f.Of(verts, q); - return DistancePtSegSqr2D(pt, vp, vq); + return DistancePtSegSqr2D(pt, vp, vq, out t); } - public static DistSeg DistancePtSegSqr2D(Vector3f pt, Vector3f p, Vector3f q) + public static float DistancePtSegSqr2D(Vector3f pt, Vector3f p, Vector3f q, out float t) { float pqx = q.x - p.x; float pqz = q.z - p.z; float dx = pt.x - p.x; float dz = pt.z - p.z; float d = pqx * pqx + pqz * pqz; - float t = pqx * dx + pqz * dz; + t = pqx * dx + pqz * dz; if (d > 0) { t /= d; @@ -173,11 +173,7 @@ namespace DotRecast.Core dx = p.x + t * pqx - pt.x; dz = p.z + t * pqz - pt.z; - return new DistSeg() - { - DistSqr = dx * dx + dz * dz, - Seg = t, - }; + return dx * dx + dz * dz; } public static float? ClosestHeightPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c) @@ -244,15 +240,13 @@ namespace DotRecast.Core { int vi = i * 3; int vj = j * 3; - if (((verts[vi + 2] > pt.z) != (verts[vj + 2] > pt.z)) && (pt.x < (verts[vj + 0] - verts[vi + 0]) - * (pt.z - verts[vi + 2]) / (verts[vj + 2] - verts[vi + 2]) + verts[vi + 0])) + if (((verts[vi + 2] > pt.z) != (verts[vj + 2] > pt.z)) && + (pt.x < (verts[vj + 0] - verts[vi + 0]) * (pt.z - verts[vi + 2]) / (verts[vj + 2] - verts[vi + 2]) + verts[vi + 0])) { c = !c; } - var edet = DistancePtSegSqr2D(pt, verts, vj, vi); - ed[j] = edet.DistSqr; - et[j] = edet.Seg; + ed[j] = DistancePtSegSqr2D(pt, verts, vj, vi, out et[j]); } return c; @@ -471,20 +465,24 @@ namespace DotRecast.Core } - public static Tuple IntersectSegSeg2D(Vector3f ap, Vector3f aq, Vector3f bp, Vector3f bq) + public static bool IntersectSegSeg2D(Vector3f ap, Vector3f aq, Vector3f bp, Vector3f bq, out float s, out float t) { + s = 0; + t = 0; + Vector3f u = aq.Subtract(ap); Vector3f v = bq.Subtract(bp); Vector3f w = ap.Subtract(bp); float d = Vector3f.PerpXZ(u, v); if (Math.Abs(d) < 1e-6f) { - return null; + return false; } - float s = Vector3f.PerpXZ(v, w) / d; - float t = Vector3f.PerpXZ(u, w) / d; - return Tuple.Create(s, t); + s = Vector3f.PerpXZ(v, w) / d; + t = Vector3f.PerpXZ(u, w) / d; + + return true; } } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/LocalBoundary.cs b/src/DotRecast.Detour.Crowd/LocalBoundary.cs index 8e5996d..c72677a 100644 --- a/src/DotRecast.Detour.Crowd/LocalBoundary.cs +++ b/src/DotRecast.Detour.Crowd/LocalBoundary.cs @@ -120,13 +120,13 @@ namespace DotRecast.Detour.Crowd var s3 = Vector3f.Of(s[3], s[4], s[5]); // Skip too distant segments. - var distseg = DistancePtSegSqr2D(pos, s0, s3); - if (distseg.DistSqr > Sqr(collisionQueryRange)) + var distSqr = DistancePtSegSqr2D(pos, s0, s3, out var tseg); + if (distSqr > Sqr(collisionQueryRange)) { continue; } - AddSegment(distseg.DistSqr, s); + AddSegment(distSqr, s); } } } diff --git a/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs b/src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs index 799e52c..bb2c28d 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; - var dt = DistancePtSegSqr2D(pos, seg.p, seg.q); - seg.touch = dt.DistSqr < Sqr(r); + var distSqr = DistancePtSegSqr2D(pos, seg.p, seg.q, out var t); + seg.touch = distSqr < Sqr(r); } } diff --git a/src/DotRecast.Detour/ConvexConvexIntersection.cs b/src/DotRecast.Detour/ConvexConvexIntersection.cs index 5352bb0..38da1eb 100644 --- a/src/DotRecast.Detour/ConvexConvexIntersection.cs +++ b/src/DotRecast.Detour/ConvexConvexIntersection.cs @@ -235,11 +235,8 @@ namespace DotRecast.Detour private static Intersection SegSegInt(Vector3f a, Vector3f b, Vector3f c, Vector3f d, ref Vector3f p, ref Vector3f q) { - var isec = IntersectSegSeg2D(a, b, c, d); - if (null != isec) + if (IntersectSegSeg2D(a, b, c, d, out var s, out var t)) { - float s = isec.Item1; - float t = isec.Item2; if (s >= 0.0f && s <= 1.0f && t >= 0.0f && t <= 1.0f) { p.x = a.x + (b.x - a.x) * s; diff --git a/src/DotRecast.Detour/LegacyNavMeshQuery.cs b/src/DotRecast.Detour/LegacyNavMeshQuery.cs index 24a3fd7..c3ea3a8 100644 --- a/src/DotRecast.Detour/LegacyNavMeshQuery.cs +++ b/src/DotRecast.Detour/LegacyNavMeshQuery.cs @@ -738,10 +738,7 @@ namespace DotRecast.Detour // Calc distance to the edge. int vj = bestPoly.verts[j] * 3; int vi = bestPoly.verts[i] * 3; - var distseg = DistancePtSegSqr2D(centerPos, bestTile.data.verts, vj, vi); - float distSqr = distseg.DistSqr; - float tseg = distseg.Seg; - + var distSqr = DistancePtSegSqr2D(centerPos, bestTile.data.verts, vj, vi, out var tseg); // Edge is too far, skip. if (distSqr > radiusSqr) { @@ -784,8 +781,7 @@ namespace DotRecast.Detour // Calc distance to the edge. int va = bestPoly.verts[link.edge] * 3; int vb = bestPoly.verts[(link.edge + 1) % bestPoly.vertCount] * 3; - var distseg = DistancePtSegSqr2D(centerPos, bestTile.data.verts, va, vb); - float distSqr = distseg.DistSqr; + var distSqr = DistancePtSegSqr2D(centerPos, bestTile.data.verts, va, vb, out var tseg); // If the circle is not touching the next polygon, skip it. if (distSqr > radiusSqr) { diff --git a/src/DotRecast.Detour/NavMesh.cs b/src/DotRecast.Detour/NavMesh.cs index 49b472c..26c0131 100644 --- a/src/DotRecast.Detour/NavMesh.cs +++ b/src/DotRecast.Detour/NavMesh.cs @@ -1187,9 +1187,7 @@ namespace DotRecast.Detour continue; } - var dt = DistancePtSegSqr2D(pos, v[j], v[k]); - float d = dt.DistSqr; - float t = dt.Seg; + var d = DistancePtSegSqr2D(pos, v[j], v[k], out var t); if (d < dmin) { dmin = d; @@ -1213,9 +1211,7 @@ 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]; - var dt = DistancePtSegSqr2D(pos, v[0], v[1]); - float d = dt.DistSqr; - float t = dt.Seg; + var d = DistancePtSegSqr2D(pos, v[0], v[1], out var t); if (d < dmin) { dmin = d; @@ -1343,8 +1339,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] }; - var dt = DistancePtSegSqr2D(pos, v0, v1); - return new ClosestPointOnPolyResult(false, Vector3f.Lerp(v0, v1, dt.Seg)); + var distSqr = DistancePtSegSqr2D(pos, v0, v1, out var t); + return new ClosestPointOnPolyResult(false, Vector3f.Lerp(v0, v1, t)); } // Outside poly that is not an offmesh connection. diff --git a/src/DotRecast.Detour/NavMeshQuery.cs b/src/DotRecast.Detour/NavMeshQuery.cs index 247d7cd..7c731d9 100644 --- a/src/DotRecast.Detour/NavMeshQuery.cs +++ b/src/DotRecast.Detour/NavMeshQuery.cs @@ -346,8 +346,7 @@ namespace DotRecast.Detour var vb = portalpoints.result.right; // If the circle is not touching the next polygon, skip it. - var distseg = DistancePtSegSqr2D(centerPos, va, vb); - float distSqr = distseg.DistSqr; + var distSqr = DistancePtSegSqr2D(centerPos, va, vb, out var tesg); if (distSqr > radiusSqr) { continue; @@ -540,8 +539,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] }; - var dt = DistancePtSegSqr2D(pos, v0, v1); - return Results.Success(v0.y + (v1.y - v0.y) * dt.Seg); + var distSqr = DistancePtSegSqr2D(pos, v0, v1, out var tseg); + return Results.Success(v0.y + (v1.y - v0.y) * tseg); } float? height = m_nav.GetPolyHeight(tile, poly, pos); @@ -1500,10 +1499,8 @@ namespace DotRecast.Detour } // Append intersection - Tuple interect = IntersectSegSeg2D(startPos, endPos, left, right); - if (null != interect) + if (IntersectSegSeg2D(startPos, endPos, left, right, out var _, out var t)) { - float t = interect.Item2; var pt = Vector3f.Lerp(left, right, t); stat = AppendVertex(pt, 0, path[i + 1], straightPath, maxStraightPath); if (!stat.IsInProgress()) @@ -1626,8 +1623,8 @@ namespace DotRecast.Detour // If starting really close the portal, advance. if (i == 0) { - var dt = DistancePtSegSqr2D(portalApex, left, right); - if (dt.DistSqr < Sqr(0.001f)) + var distSqr = DistancePtSegSqr2D(portalApex, left, right, out var t); + if (distSqr < Sqr(0.001f)) { continue; } @@ -1906,9 +1903,7 @@ namespace DotRecast.Detour // Wall edge, calc distance. int vj = j * 3; int vi = i * 3; - var distSeg = DistancePtSegSqr2D(endPos, verts, vj, vi); - float distSqr = distSeg.DistSqr; - float tseg = distSeg.Seg; + var distSqr = DistancePtSegSqr2D(endPos, verts, vj, vi, out var tseg); if (distSqr < bestDist) { // Update nearest distance. @@ -1932,8 +1927,7 @@ namespace DotRecast.Detour // TODO: Maybe should use GetPortalPoints(), but this one is way faster. int vj = j * 3; int vi = i * 3; - var distseg = DistancePtSegSqr2D(searchPos, verts, vj, vi); - float distSqr = distseg.DistSqr; + var distSqr = DistancePtSegSqr2D(searchPos, verts, vj, vi, out var _); if (distSqr > searchRadSqr) { continue; @@ -2127,17 +2121,16 @@ namespace DotRecast.Detour Vector3f left = ppoints.result.left; Vector3f right = ppoints.result.right; float t = 0.5f; - Tuple interect = IntersectSegSeg2D(fromPos, toPos, left, right); - if (null != interect) + if (IntersectSegSeg2D(fromPos, toPos, left, right, out var _, out var t2)) { - t = Clamp(interect.Item2, 0.1f, 0.9f); + t = Clamp(t2, 0.1f, 0.9f); } Vector3f pt = Vector3f.Lerp(left, right, t); return Results.Success(pt); } - private static float s = 1.0f / 255.0f; + private const float s = 1.0f / 255.0f; /// @par /// @@ -2553,8 +2546,7 @@ namespace DotRecast.Detour var vb = pp.result.right; // If the circle is not touching the next polygon, skip it. - var distseg = DistancePtSegSqr2D(centerPos, va, vb); - float distSqr = distseg.DistSqr; + var distSqr = DistancePtSegSqr2D(centerPos, va, vb, out var _); if (distSqr > radiusSqr) { continue; @@ -2917,8 +2909,7 @@ namespace DotRecast.Detour var vb = pp.result.right; // If the circle is not touching the next polygon, skip it. - var distseg = DistancePtSegSqr2D(centerPos, va, vb); - float distSqr = distseg.DistSqr; + var distSqr = DistancePtSegSqr2D(centerPos, va, vb, out var _); if (distSqr > radiusSqr) { continue; @@ -3261,9 +3252,7 @@ namespace DotRecast.Detour // Calc distance to the edge. int vj = bestPoly.verts[j] * 3; int vi = bestPoly.verts[i] * 3; - var distseg = DistancePtSegSqr2D(centerPos, bestTile.data.verts, vj, vi); - float distSqr = distseg.DistSqr; - float tseg = distseg.Seg; + var distSqr = DistancePtSegSqr2D(centerPos, bestTile.data.verts, vj, vi, out var tseg); // Edge is too far, skip. if (distSqr > radiusSqr) @@ -3307,8 +3296,7 @@ namespace DotRecast.Detour // Calc distance to the edge. int va = bestPoly.verts[link.edge] * 3; int vb = bestPoly.verts[(link.edge + 1) % bestPoly.vertCount] * 3; - var distseg = DistancePtSegSqr2D(centerPos, bestTile.data.verts, va, vb); - float distSqr = distseg.DistSqr; + var distSqr = DistancePtSegSqr2D(centerPos, bestTile.data.verts, va, vb, out var tseg); // If the circle is not touching the next polygon, skip it. if (distSqr > radiusSqr) { diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index 94ffa57..8139e60 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -850,8 +850,8 @@ public class TestNavmeshTool : Tool var v0 = Vector3f.Of(s[0], s[1], s[2]); var s3 = Vector3f.Of(s[3], s[4], s[5]); // Skip too distant segments. - var distSqr = DistancePtSegSqr2D(m_spos, v0, s3); - if (distSqr.DistSqr > RcMath.Sqr(m_neighbourhoodRadius)) + var distSqr = DistancePtSegSqr2D(m_spos, v0, s3, out var tseg); + if (distSqr > RcMath.Sqr(m_neighbourhoodRadius)) { continue; }