bit
/
DotRecastNetSim
forked from mirror/DotRecast
40
0
Fork 1
Code Pull Requests Releases Activity

remove tuple<float, float> ok!!

This commit is contained in:
ikpil 2023-05-30 22:15:44 +09:00
parent 98f62a4f8a
commit bf07c8f3ff
9 changed files with 46 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/DotRecast.Core/DistSeg.cs
View File

@ -1,8 +0,0 @@
namespace DotRecast.Core
{
public struct DistSeg
{
public float DistSqr;
public float Seg;
}
}

36
src/DotRecast.Core/RcMath.cs
View File

@ -142,21 +142,21 @@ namespace DotRecast.Core
return overlap; 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 vp = Vector3f.Of(verts, p);
var vq = Vector3f.Of(verts, q); 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 pqx = q.x - p.x;
float pqz = q.z - p.z; float pqz = q.z - p.z;
float dx = pt.x - p.x; float dx = pt.x - p.x;
float dz = pt.z - p.z; float dz = pt.z - p.z;
float d = pqx * pqx + pqz * pqz; float d = pqx * pqx + pqz * pqz;
float t = pqx * dx + pqz * dz; t = pqx * dx + pqz * dz;
if (d > 0) if (d > 0)
{ {
t /= d; t /= d;
@ -173,11 +173,7 @@ namespace DotRecast.Core
dx = p.x + t * pqx - pt.x; dx = p.x + t * pqx - pt.x;
dz = p.z + t * pqz - pt.z; dz = p.z + t * pqz - pt.z;
return new DistSeg() return dx * dx + dz * dz;
{
DistSqr = dx * dx + dz * dz,
Seg = t,
};
} }
public static float? ClosestHeightPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c) public static float? ClosestHeightPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c)
@ -244,15 +240,13 @@ namespace DotRecast.Core
{ {
int vi = i * 3; int vi = i * 3;
int vj = j * 3; int vj = j * 3;
if (((verts[vi + 2] > pt.z) != (verts[vj + 2] > pt.z)) && (pt.x < (verts[vj + 0] - verts[vi + 0]) if (((verts[vi + 2] > pt.z) != (verts[vj + 2] > pt.z)) &&
* (pt.z - verts[vi + 2]) / (verts[vj + 2] - verts[vi + 2]) + verts[vi + 0])) (pt.x < (verts[vj + 0] - verts[vi + 0]) * (pt.z - verts[vi + 2]) / (verts[vj + 2] - verts[vi + 2]) + verts[vi + 0]))
{ {
c = !c; c = !c;
} }
var edet = DistancePtSegSqr2D(pt, verts, vj, vi); ed[j] = DistancePtSegSqr2D(pt, verts, vj, vi, out et[j]);
ed[j] = edet.DistSqr;
et[j] = edet.Seg;
} }
return c; return c;
@ -471,20 +465,24 @@ namespace DotRecast.Core
} }
public static Tuple<float, float> 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 u = aq.Subtract(ap);
Vector3f v = bq.Subtract(bp); Vector3f v = bq.Subtract(bp);
Vector3f w = ap.Subtract(bp); Vector3f w = ap.Subtract(bp);
float d = Vector3f.PerpXZ(u, v); float d = Vector3f.PerpXZ(u, v);
if (Math.Abs(d) < 1e-6f) if (Math.Abs(d) < 1e-6f)
{ {
return null; return false;
} }
float s = Vector3f.PerpXZ(v, w) / d; s = Vector3f.PerpXZ(v, w) / d;
float t = Vector3f.PerpXZ(u, w) / d; t = Vector3f.PerpXZ(u, w) / d;
return Tuple.Create(s, t);
return true;
} }
} }
} }

6
src/DotRecast.Detour.Crowd/LocalBoundary.cs
View File

@ -120,13 +120,13 @@ namespace DotRecast.Detour.Crowd
var s3 = Vector3f.Of(s[3], s[4], s[5]); var s3 = Vector3f.Of(s[3], s[4], s[5]);
// Skip too distant segments. // Skip too distant segments.
var distseg = DistancePtSegSqr2D(pos, s0, s3); var distSqr = DistancePtSegSqr2D(pos, s0, s3, out var tseg);
if (distseg.DistSqr > Sqr(collisionQueryRange)) if (distSqr > Sqr(collisionQueryRange))
{ {
continue; continue;
} }
AddSegment(distseg.DistSqr, s); AddSegment(distSqr, s);
} }
} }
} }

4
src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs
View File

@ -150,8 +150,8 @@ namespace DotRecast.Detour.Crowd
// Precalc if the agent is really close to the segment. // Precalc if the agent is really close to the segment.
float r = 0.01f; float r = 0.01f;
var dt = DistancePtSegSqr2D(pos, seg.p, seg.q); var distSqr = DistancePtSegSqr2D(pos, seg.p, seg.q, out var t);
seg.touch = dt.DistSqr < Sqr(r); seg.touch = distSqr < Sqr(r);
} }
} }

5
src/DotRecast.Detour/ConvexConvexIntersection.cs
View File

@ -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) 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 (IntersectSegSeg2D(a, b, c, d, out var s, out var t))
if (null != isec)
{ {
float s = isec.Item1;
float t = isec.Item2;
if (s >= 0.0f && s <= 1.0f && t >= 0.0f && t <= 1.0f) if (s >= 0.0f && s <= 1.0f && t >= 0.0f && t <= 1.0f)
{ {
p.x = a.x + (b.x - a.x) * s; p.x = a.x + (b.x - a.x) * s;

8
src/DotRecast.Detour/LegacyNavMeshQuery.cs
View File

@ -738,10 +738,7 @@ namespace DotRecast.Detour
// Calc distance to the edge. // Calc distance to the edge.
int vj = bestPoly.verts[j] * 3; int vj = bestPoly.verts[j] * 3;
int vi = bestPoly.verts[i] * 3; int vi = bestPoly.verts[i] * 3;
var distseg = DistancePtSegSqr2D(centerPos, bestTile.data.verts, vj, vi); var distSqr = DistancePtSegSqr2D(centerPos, bestTile.data.verts, vj, vi, out var tseg);
float distSqr = distseg.DistSqr;
float tseg = distseg.Seg;
// Edge is too far, skip. // Edge is too far, skip.
if (distSqr > radiusSqr) if (distSqr > radiusSqr)
{ {
@ -784,8 +781,7 @@ namespace DotRecast.Detour
// Calc distance to the edge. // Calc distance to the edge.
int va = bestPoly.verts[link.edge] * 3; int va = bestPoly.verts[link.edge] * 3;
int vb = bestPoly.verts[(link.edge + 1) % bestPoly.vertCount] * 3; int vb = bestPoly.verts[(link.edge + 1) % bestPoly.vertCount] * 3;
var distseg = DistancePtSegSqr2D(centerPos, bestTile.data.verts, va, vb); var distSqr = DistancePtSegSqr2D(centerPos, bestTile.data.verts, va, vb, out var tseg);
float distSqr = distseg.DistSqr;
// If the circle is not touching the next polygon, skip it. // If the circle is not touching the next polygon, skip it.
if (distSqr > radiusSqr) if (distSqr > radiusSqr)
{ {

12
src/DotRecast.Detour/NavMesh.cs
View File

@ -1187,9 +1187,7 @@ namespace DotRecast.Detour
continue; continue;
} }
var dt = DistancePtSegSqr2D(pos, v[j], v[k]); var d = DistancePtSegSqr2D(pos, v[j], v[k], out var t);
float d = dt.DistSqr;
float t = dt.Seg;
if (d < dmin) if (d < dmin)
{ {
dmin = d; dmin = d;
@ -1213,9 +1211,7 @@ namespace DotRecast.Detour
v[1].y = tile.data.verts[poly.verts[k] * 3 + 1]; v[1].y = tile.data.verts[poly.verts[k] * 3 + 1];
v[1].z = tile.data.verts[poly.verts[k] * 3 + 2]; v[1].z = tile.data.verts[poly.verts[k] * 3 + 2];
var dt = DistancePtSegSqr2D(pos, v[0], v[1]); var d = DistancePtSegSqr2D(pos, v[0], v[1], out var t);
float d = dt.DistSqr;
float t = dt.Seg;
if (d < dmin) if (d < dmin)
{ {
dmin = d; 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] }; 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; 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 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); var distSqr = DistancePtSegSqr2D(pos, v0, v1, out var t);
return new ClosestPointOnPolyResult(false, Vector3f.Lerp(v0, v1, dt.Seg)); return new ClosestPointOnPolyResult(false, Vector3f.Lerp(v0, v1, t));
} }
// Outside poly that is not an offmesh connection. // Outside poly that is not an offmesh connection.

42
src/DotRecast.Detour/NavMeshQuery.cs
View File

@ -346,8 +346,7 @@ namespace DotRecast.Detour
var vb = portalpoints.result.right; var vb = portalpoints.result.right;
// If the circle is not touching the next polygon, skip it. // If the circle is not touching the next polygon, skip it.
var distseg = DistancePtSegSqr2D(centerPos, va, vb); var distSqr = DistancePtSegSqr2D(centerPos, va, vb, out var tesg);
float distSqr = distseg.DistSqr;
if (distSqr > radiusSqr) if (distSqr > radiusSqr)
{ {
continue; 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] }; 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; 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 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); var distSqr = DistancePtSegSqr2D(pos, v0, v1, out var tseg);
return Results.Success(v0.y + (v1.y - v0.y) * dt.Seg); return Results.Success(v0.y + (v1.y - v0.y) * tseg);
} }
float? height = m_nav.GetPolyHeight(tile, poly, pos); float? height = m_nav.GetPolyHeight(tile, poly, pos);
@ -1500,10 +1499,8 @@ namespace DotRecast.Detour
} }
// Append intersection // Append intersection
Tuple<float, float> interect = IntersectSegSeg2D(startPos, endPos, left, right); if (IntersectSegSeg2D(startPos, endPos, left, right, out var _, out var t))
if (null != interect)
{ {
float t = interect.Item2;
var pt = Vector3f.Lerp(left, right, t); var pt = Vector3f.Lerp(left, right, t);
stat = AppendVertex(pt, 0, path[i + 1], straightPath, maxStraightPath); stat = AppendVertex(pt, 0, path[i + 1], straightPath, maxStraightPath);
if (!stat.IsInProgress()) if (!stat.IsInProgress())
@ -1626,8 +1623,8 @@ namespace DotRecast.Detour
// If starting really close the portal, advance. // If starting really close the portal, advance.
if (i == 0) if (i == 0)
{ {
var dt = DistancePtSegSqr2D(portalApex, left, right); var distSqr = DistancePtSegSqr2D(portalApex, left, right, out var t);
if (dt.DistSqr < Sqr(0.001f)) if (distSqr < Sqr(0.001f))
{ {
continue; continue;
} }
@ -1906,9 +1903,7 @@ namespace DotRecast.Detour
// Wall edge, calc distance. // Wall edge, calc distance.
int vj = j * 3; int vj = j * 3;
int vi = i * 3; int vi = i * 3;
var distSeg = DistancePtSegSqr2D(endPos, verts, vj, vi); var distSqr = DistancePtSegSqr2D(endPos, verts, vj, vi, out var tseg);
float distSqr = distSeg.DistSqr;
float tseg = distSeg.Seg;
if (distSqr < bestDist) if (distSqr < bestDist)
{ {
// Update nearest distance. // Update nearest distance.
@ -1932,8 +1927,7 @@ namespace DotRecast.Detour
// TODO: Maybe should use GetPortalPoints(), but this one is way faster. // TODO: Maybe should use GetPortalPoints(), but this one is way faster.
int vj = j * 3; int vj = j * 3;
int vi = i * 3; int vi = i * 3;
var distseg = DistancePtSegSqr2D(searchPos, verts, vj, vi); var distSqr = DistancePtSegSqr2D(searchPos, verts, vj, vi, out var _);
float distSqr = distseg.DistSqr;
if (distSqr > searchRadSqr) if (distSqr > searchRadSqr)
{ {
continue; continue;
@ -2127,17 +2121,16 @@ namespace DotRecast.Detour
Vector3f left = ppoints.result.left; Vector3f left = ppoints.result.left;
Vector3f right = ppoints.result.right; Vector3f right = ppoints.result.right;
float t = 0.5f; float t = 0.5f;
Tuple<float, float> interect = IntersectSegSeg2D(fromPos, toPos, left, right); if (IntersectSegSeg2D(fromPos, toPos, left, right, out var _, out var t2))
if (null != interect)
{ {
t = Clamp(interect.Item2, 0.1f, 0.9f); t = Clamp(t2, 0.1f, 0.9f);
} }
Vector3f pt = Vector3f.Lerp(left, right, t); Vector3f pt = Vector3f.Lerp(left, right, t);
return Results.Success(pt); return Results.Success(pt);
} }
private static float s = 1.0f / 255.0f; private const float s = 1.0f / 255.0f;
/// @par /// @par
/// ///
@ -2553,8 +2546,7 @@ namespace DotRecast.Detour
var vb = pp.result.right; var vb = pp.result.right;
// If the circle is not touching the next polygon, skip it. // If the circle is not touching the next polygon, skip it.
var distseg = DistancePtSegSqr2D(centerPos, va, vb); var distSqr = DistancePtSegSqr2D(centerPos, va, vb, out var _);
float distSqr = distseg.DistSqr;
if (distSqr > radiusSqr) if (distSqr > radiusSqr)
{ {
continue; continue;
@ -2917,8 +2909,7 @@ namespace DotRecast.Detour
var vb = pp.result.right; var vb = pp.result.right;
// If the circle is not touching the next polygon, skip it. // If the circle is not touching the next polygon, skip it.
var distseg = DistancePtSegSqr2D(centerPos, va, vb); var distSqr = DistancePtSegSqr2D(centerPos, va, vb, out var _);
float distSqr = distseg.DistSqr;
if (distSqr > radiusSqr) if (distSqr > radiusSqr)
{ {
continue; continue;
@ -3261,9 +3252,7 @@ namespace DotRecast.Detour
// Calc distance to the edge. // Calc distance to the edge.
int vj = bestPoly.verts[j] * 3; int vj = bestPoly.verts[j] * 3;
int vi = bestPoly.verts[i] * 3; int vi = bestPoly.verts[i] * 3;
var distseg = DistancePtSegSqr2D(centerPos, bestTile.data.verts, vj, vi); var distSqr = DistancePtSegSqr2D(centerPos, bestTile.data.verts, vj, vi, out var tseg);
float distSqr = distseg.DistSqr;
float tseg = distseg.Seg;
// Edge is too far, skip. // Edge is too far, skip.
if (distSqr > radiusSqr) if (distSqr > radiusSqr)
@ -3307,8 +3296,7 @@ namespace DotRecast.Detour
// Calc distance to the edge. // Calc distance to the edge.
int va = bestPoly.verts[link.edge] * 3; int va = bestPoly.verts[link.edge] * 3;
int vb = bestPoly.verts[(link.edge + 1) % bestPoly.vertCount] * 3; int vb = bestPoly.verts[(link.edge + 1) % bestPoly.vertCount] * 3;
var distseg = DistancePtSegSqr2D(centerPos, bestTile.data.verts, va, vb); var distSqr = DistancePtSegSqr2D(centerPos, bestTile.data.verts, va, vb, out var tseg);
float distSqr = distseg.DistSqr;
// If the circle is not touching the next polygon, skip it. // If the circle is not touching the next polygon, skip it.
if (distSqr > radiusSqr) if (distSqr > radiusSqr)
{ {

4
src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs
View File

@ -850,8 +850,8 @@ public class TestNavmeshTool : Tool
var v0 = Vector3f.Of(s[0], s[1], s[2]); var v0 = Vector3f.Of(s[0], s[1], s[2]);
var s3 = Vector3f.Of(s[3], s[4], s[5]); var s3 = Vector3f.Of(s[3], s[4], s[5]);
// Skip too distant segments. // Skip too distant segments.
var distSqr = DistancePtSegSqr2D(m_spos, v0, s3); var distSqr = DistancePtSegSqr2D(m_spos, v0, s3, out var tseg);
if (distSqr.DistSqr > RcMath.Sqr(m_neighbourhoodRadius)) if (distSqr > RcMath.Sqr(m_neighbourhoodRadius))
{ {
continue; continue;
} }