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

Intersection query alloc fix

This commit is contained in:
wrenge 2024-11-14 12:40:03 +03:00
parent 12e09475f0
commit 088edcd655
8 changed files with 74 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/DotRecast.Core/Numerics/RcVec.cs
View File

@ -205,7 +205,7 @@ namespace DotRecast.Core.Numerics
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dist2DSqr(RcVec3f p, float[] verts, int i)
public static float Dist2DSqr(RcVec3f p, Span<float> verts, int i)
{
float dx = verts[i] - p.X;
float dz = verts[i + 2] - p.Z;

9
src/DotRecast.Detour/DtConvexConvexIntersections.cs
View File

@ -27,7 +27,7 @@ namespace DotRecast.Detour
{
private const float EPSILON = 0.0001f;
public static float[] Intersect(Span<float> p, Span<float> q)
public static Span<float> Intersect(Span<float> p, Span<float> q, Span<float> buffer)
{
int n = p.Length / 3;
int m = q.Length / 3;
@ -95,7 +95,7 @@ namespace DotRecast.Detour
/* Special case: A & B parallel and separated. */
if (parallel && aHB < 0f && bHA < 0f)
{
return null;
return Span<float>.Empty;
}
/* Special case: A & B collinear. */
else if (parallel && MathF.Abs(aHB) < EPSILON && MathF.Abs(bHA) < EPSILON)
@ -168,8 +168,9 @@ namespace DotRecast.Detour
return null;
}
float[] copied = inters.Slice(0, ii).ToArray();
return copied;
Span<float> result = buffer.Slice(0, ii);
inters.Slice(0, ii).CopyTo(result);
return result;
}
private static int AddVertex(Span<float> inters, int ii, RcVec3f p)

18
src/DotRecast.Detour/DtNavMeshQuery.cs
View File

@ -233,6 +233,7 @@ namespace DotRecast.Detour
IDtQueryFilter filter, IRcRand frand, IDtPolygonByCircleConstraint constraint,
out long randomRef, out RcVec3f randomPt)
{
const int MAX_VERT_BUFFER_SIZE = 128;
randomRef = startRef;
randomPt = centerPos;
@ -266,9 +267,13 @@ namespace DotRecast.Detour
float radiusSqr = maxRadius * maxRadius;
float areaSum = 0.0f;
using RcRentedArray<float> polyVertsBuffer = RcRentedArray.Rent<float>(MAX_VERT_BUFFER_SIZE);
using RcRentedArray<float> randomPolyVertsBuffer = RcRentedArray.Rent<float>(MAX_VERT_BUFFER_SIZE);
using RcRentedArray<float> constrainedVertsBuffer = RcRentedArray.Rent<float>(MAX_VERT_BUFFER_SIZE);
DtPoly randomPoly = null;
long randomPolyRef = 0;
float[] randomPolyVerts = null;
Span<float> randomPolyVerts = Span<float>.Empty;
while (!m_openList.IsEmpty())
{
@ -286,14 +291,14 @@ namespace DotRecast.Detour
{
// Calc area of the polygon.
float polyArea = 0.0f;
float[] polyVerts = new float[bestPoly.vertCount * 3];
Span<float> polyVerts = polyVertsBuffer.AsSpan().Slice(0, bestPoly.vertCount * 3);
for (int j = 0; j < bestPoly.vertCount; ++j)
{
RcArrays.Copy(bestTile.data.verts, bestPoly.verts[j] * 3, polyVerts, j * 3, 3);
RcSpans.Copy(bestTile.data.verts, bestPoly.verts[j] * 3, polyVerts, j * 3, 3);
}
float[] constrainedVerts = constraint.Apply(polyVerts, centerPos, maxRadius);
if (constrainedVerts != null)
Span<float> constrainedVerts = constraint.Apply(polyVerts, centerPos, maxRadius, constrainedVertsBuffer.AsSpan());
if (!constrainedVerts.IsEmpty)
{
int vertCount = constrainedVerts.Length / 3;
for (int j = 2; j < vertCount; ++j)
@ -311,7 +316,8 @@ namespace DotRecast.Detour
{
randomPoly = bestPoly;
randomPolyRef = bestRef;
randomPolyVerts = constrainedVerts;
randomPolyVerts = randomPolyVertsBuffer.AsSpan().Slice(0, constrainedVerts.Length);
constrainedVerts.CopyTo(randomPolyVerts);
}
}
}

7
src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs
View File

@ -1,3 +1,4 @@
using System;
using DotRecast.Core.Numerics;
namespace DotRecast.Detour
@ -10,9 +11,11 @@ namespace DotRecast.Detour
{
}
public float[] Apply(float[] polyVerts, RcVec3f circleCenter, float radius)
public Span<float> Apply(Span<float> polyVerts, RcVec3f circleCenter, float radius, Span<float> resultBuffer)
{
return polyVerts;
var result = resultBuffer.Slice(0, polyVerts.Length);
polyVerts.CopyTo(result);
return result;
}
}
}

24
src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs
View File

@ -39,8 +39,7 @@ namespace DotRecast.Detour
}
}
public float[] Apply(float[] verts, RcVec3f center, float radius)
public Span<float> Apply(Span<float> verts, RcVec3f center, float radius, Span<float> resultBuffer)
{
float radiusSqr = radius * radius;
int outsideVertex = -1;
@ -56,19 +55,30 @@ namespace DotRecast.Detour
if (outsideVertex == -1)
{
// polygon inside circle
return verts;
var result = resultBuffer.Slice(0, verts.Length);
verts.CopyTo(result);
return result;
}
Span<float> qCircle = stackalloc float[UnitCircle.Length];
ScaleCircle(UnitCircle, center, radius, qCircle);
float[] intersection = DtConvexConvexIntersections.Intersect(verts, qCircle);
if (intersection == null && DtUtils.PointInPolygon(center, verts, verts.Length / 3))
Span<float> intersection = DtConvexConvexIntersections.Intersect(verts, qCircle, resultBuffer);
if (intersection.IsEmpty && DtUtils.PointInPolygon(center, verts, verts.Length / 3))
{
// circle inside polygon
return qCircle.ToArray();
var result = resultBuffer.Slice(0, qCircle.Length);
qCircle.CopyTo(result);
return result;
}
return intersection;
if(!intersection.IsEmpty)
{
var result = resultBuffer.Slice(0, intersection.Length);
// No need to copy, data is already in buffer
return result;
}
return Span<float>.Empty;
}
}
}

2
src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs
View File

@ -24,6 +24,6 @@ namespace DotRecast.Detour
{
public interface IDtPolygonByCircleConstraint
{
float[] Apply(float[] polyVerts, RcVec3f circleCenter, float radius);
Span<float> Apply(Span<float> polyVerts, RcVec3f circleCenter, float radius, Span<float> resultBuffer);
}
}

11
test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs
View File

@ -17,6 +17,7 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using System;
using NUnit.Framework;
namespace DotRecast.Detour.Test;
@ -29,9 +30,10 @@ public class ConvexConvexIntersectionTest
{
float[] p = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
float[] q = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
float[] intersection = DtConvexConvexIntersections.Intersect(p, q);
float[] buffer = new float[128];
Span<float> intersection = DtConvexConvexIntersections.Intersect(p, q, buffer);
Assert.That(intersection.Length, Is.EqualTo(5 * 3));
Assert.That(intersection, Is.EqualTo(p));
Assert.That(intersection.ToArray(), Is.EquivalentTo(p));
}
[Test]
@ -39,8 +41,9 @@ public class ConvexConvexIntersectionTest
{
float[] p = { -5, 0, -5, -5, 0, 4, 1, 0, 4, 1, 0, -5 };
float[] q = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
float[] intersection = DtConvexConvexIntersections.Intersect(p, q);
float[] buffer = new float[128];
Span<float> intersection = DtConvexConvexIntersections.Intersect(p, q, buffer);
Assert.That(intersection.Length, Is.EqualTo(5 * 3));
Assert.That(intersection, Is.EqualTo(new[] { 1, 0, 3, 1, 0, -3.4f, -2, 0, -4, -4, 0, 0, -3, 0, 3 }));
Assert.That(intersection.ToArray(), Is.EquivalentTo(new[] { 1, 0, 3, 1, 0, -3.4f, -2, 0, -4, -4, 0, 0, -3, 0, 3 }));
}
}

34
test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs
View File

@ -17,6 +17,7 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using System;
using DotRecast.Core.Numerics;
using NUnit.Framework;
@ -32,9 +33,12 @@ public class PolygonByCircleConstraintTest
{
float[] polygon = { -2, 0, 2, 2, 0, 2, 2, 0, -2, -2, 0, -2 };
RcVec3f center = new RcVec3f(1, 0, 1);
float[] constrained = _constraint.Apply(polygon, center, 6);
var radius = 6;
Assert.That(constrained, Is.EqualTo(polygon));
float[] buffer = new float[128];
Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
Assert.That(constrained.ToArray(), Is.EquivalentTo(polygon));
}
[Test]
@ -43,10 +47,13 @@ public class PolygonByCircleConstraintTest
int expectedSize = 21;
float[] polygon = { -2, 0, 2, 2, 0, 2, 2, 0, -2, -2, 0, -2 };
RcVec3f center = new RcVec3f(2, 0, 0);
var radius = 3;
float[] buffer = new float[128];
Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
float[] constrained = _constraint.Apply(polygon, center, 3);
Assert.That(constrained.Length, Is.EqualTo(expectedSize));
Assert.That(constrained, Is.SupersetOf(new[] { 2f, 0f, 2f, 2f, 0f, -2f }));
Assert.That(constrained.ToArray(), Is.SupersetOf(new[] { 2f, 0f, 2f, 2f, 0f, -2f }));
}
[Test]
@ -55,7 +62,10 @@ public class PolygonByCircleConstraintTest
int expectedSize = 12 * 3;
float[] polygon = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
RcVec3f center = new RcVec3f(-1, 0, -1);
float[] constrained = _constraint.Apply(polygon, center, 2);
var radius = 2;
float[] buffer = new float[128];
Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
Assert.That(constrained.Length, Is.EqualTo(expectedSize));
@ -73,10 +83,13 @@ public class PolygonByCircleConstraintTest
int expectedSize = 9 * 3;
float[] polygon = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
RcVec3f center = new RcVec3f(-2, 0, -1);
float[] constrained = _constraint.Apply(polygon, center, 3);
var radius = 3;
float[] buffer = new float[128];
Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
Assert.That(constrained.Length, Is.EqualTo(expectedSize));
Assert.That(constrained, Is.SupersetOf(new[] { -2f, 0f, -4f, -4f, 0f, 0f, -3.4641016f, 0.0f, 1.60769534f, -2.0f, 0.0f, 2.0f }));
Assert.That(constrained.ToArray(), Is.SupersetOf(new[] { -2f, 0f, -4f, -4f, 0f, 0f, -3.4641016f, 0.0f, 1.60769534f, -2.0f, 0.0f, 2.0f }));
}
[Test]
@ -85,9 +98,12 @@ public class PolygonByCircleConstraintTest
int expectedSize = 7 * 3;
float[] polygon = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
RcVec3f center = new RcVec3f(4, 0, 0);
float[] constrained = _constraint.Apply(polygon, center, 4);
var radius = 4;
float[] buffer = new float[128];
Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
Assert.That(constrained.Length, Is.EqualTo(expectedSize));
Assert.That(constrained, Is.SupersetOf(new[] { 1.53589869f, 0f, 3f, 2f, 0f, 3f, 3f, 0f, -3f }));
Assert.That(constrained.ToArray(), Is.SupersetOf(new[] { 1.53589869f, 0f, 3f, 2f, 0f, 3f, 3f, 0f, -3f }));
}
}