Skip to content

Commit

Permalink
ref
Browse files Browse the repository at this point in the history
  • Loading branch information
@GabrielMotaAlexandre
GabrielMotaAlexandre committed Oct 30, 2023
1 parent e7a6f5e commit f2e45f7
Show file tree
Hide file tree
Showing 12 changed files with 175 additions and 160 deletions.
22 changes: 21 additions & 1 deletion src/DotRecast.Core/Vector3Extensions.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,13 @@ public static Vector3 Lerp(float[] verts, int v1, int v2, float t)
ref readonly Vector3 vector1 = ref verts.GetReference().UnsafeAdd(v1).UnsafeAs<float, Vector3>();
ref readonly Vector3 vector2 = ref verts.GetReference().UnsafeAdd(v2).UnsafeAs<float, Vector3>();

return vector1 + (vector2 - vector1) * t;
return Lerp(vector1, vector2, t);
}

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static Vector3 Lerp(Vector3 v1, Vector3 v2, float t)
{
return v1 + (v2 - v1) * t;
//return Vector3.Lerp(vector1, vector2, t);
}

Expand Down Expand Up @@ -237,6 +243,20 @@ public static ref T GetUnsafeNotReadonly<T>(this ReadOnlySpan<T> array, int inde
return ref array.GetReference().UnsafeAdd(index);
}

[Pure]
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static Span<TTo> Cast<T, TTo>(this Span<T> value) where T : struct where TTo : struct
{
return MemoryMarshal.Cast<T, TTo>(value);
}

[Pure]
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static ReadOnlySpan<TTo> Cast<T, TTo>(this ReadOnlySpan<T> value) where T : struct where TTo : struct
{
return MemoryMarshal.Cast<T, TTo>(value);
}

[Pure]
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static ref TTo UnsafeAs<T, TTo>(ref this T value, int index = 0) where T : struct where TTo : struct
Expand Down
81 changes: 33 additions & 48 deletions src/DotRecast.Detour/ConvexConvexIntersection.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,23 +24,19 @@ 3. This notice may not be removed or altered from any source distribution.
namespace DotRecast.Detour
{
/**
* Convex-convex intersection based on "Computational Geometry in C" by Joseph O'Rourke
*/
* Convex-convex intersection based on "Computational Geometry in C" by Joseph O'Rourke
*/
public static class ConvexConvexIntersection
{
private static readonly float EPSILON = 0.0001f;

public static float[] Intersect(ReadOnlySpan<float> p, ReadOnlySpan<float> q)
public static ReadOnlySpan<Vector3> Intersect(ReadOnlySpan<Vector3> pVerts, ReadOnlySpan<Vector3> qVerts)
{
int n = p.Length / 3;
int m = q.Length / 3;
float[] inters = new float[Math.Max(m, n) * 3 * 3];
int pLength = pVerts.Length;
int qLength = qVerts.Length;
var inters = new Vector3[Math.Max(qLength, pLength) * 3];
int ii = 0;
/* Initialize variables. */
Vector3 a = new();
Vector3 b = new();
Vector3 a1 = new();
Vector3 b1 = new();

int aa = 0;
int ba = 0;
Expand All @@ -49,29 +45,28 @@ public static float[] Intersect(ReadOnlySpan<float> p, ReadOnlySpan<float> q)

InFlag f = InFlag.Unknown;
bool FirstPoint = true;
Vector3 ip = new();
Vector3 iq = new();

do
{
a.Set(p, ai % n);
b.Set(q, bi % m);
a1.Set(p, (ai + n - 1) % n); // prev a
b1.Set(q, (bi + m - 1) % m); // prev b
var a = pVerts[ai % pLength];
var b = qVerts[bi % qLength];
var a1 = pVerts[(ai + pLength - 1) % pLength]; // prev a
var b1 = qVerts[(bi + qLength - 1) % qLength]; // prev b

Vector3 A = a - a1;
Vector3 B = b - b1;
Vector3 aV = a - a1;
Vector3 bV = b - b1;

float cross = B.X * A.Z - A.X * B.Z; // TriArea2D({0, 0}, A, B);
float cross = bV.X * aV.Z - aV.X * bV.Z; // TriArea2D({0, 0}, A, B);
float aHB = DtUtils.TriArea2D(b1, b, a);
float bHA = DtUtils.TriArea2D(a1, a, b);
if (Math.Abs(cross) < EPSILON)
if (MathF.Abs(cross) < EPSILON)
{
cross = 0f;
}

Vector3 iq = default;
bool parallel = cross is 0f;
Intersection code = parallel ? ParallelInt(a1, a, b1, b, ref ip, ref iq) : SegSegInt(a1, a, b1, b, ref ip, ref iq);
Intersection code = parallel ? ParallelInt(a1, a, b1, b, out var ip, out iq) : SegSegInt(a1, a, b1, b, out ip);

if (code == Intersection.Single)
{
Expand All @@ -94,14 +89,8 @@ public static float[] Intersect(ReadOnlySpan<float> p, ReadOnlySpan<float> q)
///
/// The vectors are projected onto the xz-plane, so the y-values are
/// ignored.
[MethodImpl(MethodImplOptions.AggressiveInlining)]
static float Dot2D(Vector3 vector, Vector3 v)
{
return vector.X * v.X + vector.Z * v.Z;
}

/* Special case: A & B overlap and oppositely oriented. */
if (code == Intersection.Overlap && Dot2D(A, B) < 0)
if (code == Intersection.Overlap && aV.X * bV.X + aV.Z * bV.Z < 0)
{
ii = AddVertex(inters, ii, ip);
ii = AddVertex(inters, ii, iq);
Expand All @@ -111,10 +100,10 @@ static float Dot2D(Vector3 vector, Vector3 v)
/* Special case: A & B parallel and separated. */
if (parallel && aHB < 0f && bHA < 0f)
{
return null;
return default;
}
/* Special case: A & B collinear. */
else if (parallel && Math.Abs(aHB) < EPSILON && Math.Abs(bHA) < EPSILON)
else if (parallel && MathF.Abs(aHB) < EPSILON && Math.Abs(bHA) < EPSILON)
{
/* Advance but do not output point. */
if (f == InFlag.Pin)
Expand Down Expand Up @@ -176,38 +165,34 @@ static float Dot2D(Vector3 vector, Vector3 v)
}
}
/* Quit when both adv. indices have cycled, or one has cycled twice. */
} while ((aa < n || ba < m) && aa < 2 * n && ba < 2 * m);
} while ((aa < pLength || ba < qLength) && aa < 2 * pLength && ba < 2 * qLength);

/* Deal with special cases: not implemented. */
if (f == InFlag.Unknown)
{
return null;
}

float[] copied = new float[ii];
Array.Copy(inters, copied, ii);
return copied;
return inters.AsSpan(0, ii);
}

private static int AddVertex(float[] inters, int ii, Vector3 p)
private static int AddVertex(Span<Vector3> inters, int ii, Vector3 p)
{
if (ii > 0)
{
if (inters[ii - 3] == p.X && inters[ii - 2] == p.Y && inters[ii - 1] == p.Z)
if (inters[ii - 1] == p)
{
return ii;
}

if (inters[0] == p.X && inters[1] == p.Y && inters[2] == p.Z)
if (inters[0] == p)
{
return ii;
}
}

inters[ii] = p.X;
inters[ii + 1] = p.Y;
inters[ii + 2] = p.Z;
return ii + 3;
inters[ii] = p;
return ii + 1;
}


Expand All @@ -225,23 +210,21 @@ private static InFlag InOut(InFlag inflag, float aHB, float bHA)
return inflag;
}

private static Intersection SegSegInt(Vector3 a, Vector3 b, Vector3 c, Vector3 d, ref Vector3 p, ref Vector3 q)
private static Intersection SegSegInt(Vector3 a, Vector3 b, Vector3 c, Vector3 d, out Vector3 p)
{
if (DtUtils.IntersectSegSeg2D(a, b, c, d, out var s, out var t))
{
if (s >= 0f && s <= 1f && t >= 0f && t <= 1f)
{
p.X = a.X + (b.X - a.X) * s;
p.Y = a.Y + (b.Y - a.Y) * s;
p.Z = a.Z + (b.Z - a.Z) * s;
p = a + (b - a) * s;
return Intersection.Single;
}
}

p = default;
return Intersection.None;
}

private static Intersection ParallelInt(Vector3 a, Vector3 b, Vector3 c, Vector3 d, ref Vector3 p, ref Vector3 q)
private static Intersection ParallelInt(Vector3 a, Vector3 b, Vector3 c, Vector3 d, out Vector3 p, out Vector3 q)
{
if (Between2D(a, b, c) && Between2D(a, b, d))
{
Expand Down Expand Up @@ -285,12 +268,14 @@ private static Intersection ParallelInt(Vector3 a, Vector3 b, Vector3 c, Vector3
return Intersection.Overlap;
}

p = q = default;

return Intersection.None;
}

private static bool Between2D(Vector3 a, Vector3 b, Vector3 c)
{
if (Math.Abs(a.X - b.X) > Math.Abs(a.Z - b.Z))
if (MathF.Abs(a.X - b.X) > MathF.Abs(a.Z - b.Z))
{
return ((a.X <= c.X) && (c.X <= b.X)) || ((a.X >= c.X) && (c.X >= b.X));
}
Expand Down
8 changes: 4 additions & 4 deletions src/DotRecast.Detour/DtNavMesh.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1047,8 +1047,8 @@ void BaseOffMeshLinks(DtMeshTile tile)
}

float[] p = con.pos; // First vertex
// findNearestPoly may return too optimistic results, further check
// to make sure.
// findNearestPoly may return too optimistic results, further check
// to make sure.
if (RcMath.Sqr(nearestPt.X - p[0]) + RcMath.Sqr(nearestPt.Z - p[2]) > RcMath.Sqr(con.rad))
{
continue;
Expand Down Expand Up @@ -1195,11 +1195,11 @@ public bool GetPolyHeight(DtMeshTile tile, DtPoly poly, Vector3 pos, out float h

int ip = poly.index;

float[] verts = new float[m_maxVertPerPoly * 3];
var verts = new Vector3[m_maxVertPerPoly];
int nv = poly.vertCount;
for (int i = 0; i < nv; ++i)
{
Array.Copy(tile.data.verts, poly.verts[i] * 3, verts, i * 3, 3);
verts[i] = tile.data.verts.UnsafeAs<float, Vector3>(poly.verts[i]);
}

if (!DtUtils.PointInPolygon(pos, verts, nv))
Expand Down
46 changes: 22 additions & 24 deletions src/DotRecast.Detour/DtNavMeshQuery.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@ 3. This notice may not be removed or altered from any source distribution.

namespace DotRecast.Detour
{

using static DtNode;

public sealed class DtNavMeshQuery
Expand Down Expand Up @@ -163,17 +162,17 @@ public DtStatus FindRandomPoint(IDtQueryFilter filter, IRcRand frand, out long r
}

// Randomly pick point on polygon.
var verts = new float[3 * m_nav.GetMaxVertsPerPoly()];
var areas = new float[m_nav.GetMaxVertsPerPoly()];
Array.Copy(tile.data.verts, poly.verts[0] * 3, verts, 0, 3);
for (int j = 1; j < poly.vertCount; ++j)
var verts = new Vector3[m_nav.GetMaxVertsPerPoly()];
for (int j = 0; j < poly.vertCount; ++j)
{
Array.Copy(tile.data.verts, poly.verts[j] * 3, verts, j * 3, 3);
verts[j] = tile.data.verts.UnsafeAs<float, Vector3>(poly.verts[j]);
}

float s = frand.Next();
float t = frand.Next();

var areas = new float[m_nav.GetMaxVertsPerPoly()];

var pt = DtUtils.RandomPointInConvexPoly(verts, poly.vertCount, areas, s, t);
ClosestPointOnPoly(polyRef, pt, out var closest, out var _);

Expand Down Expand Up @@ -265,7 +264,7 @@ public DtStatus FindRandomPointAroundCircle(long startRef, Vector3 centerPos, fl

DtPoly randomPoly = null;
long randomPolyRef = 0;
ReadOnlySpan<float> randomPolyVerts = null;
ReadOnlySpan<Vector3> randomPolyVerts = default;

while (!m_openList.IsEmpty())
{
Expand All @@ -281,22 +280,23 @@ public DtStatus FindRandomPointAroundCircle(long startRef, Vector3 centerPos, fl
if (bestPoly.GetPolyType() == DtPoly.DT_POLYTYPE_GROUND)
{
// Calc area of the polygon.
float polyArea = 0f;
float[] polyVerts = new float[bestPoly.vertCount * 3];
var polyVerts = new Vector3[bestPoly.vertCount];
for (int j = 0; j < bestPoly.vertCount; ++j)
{
Array.Copy(bestTile.data.verts, bestPoly.verts[j] * 3, polyVerts, j * 3, 3);
polyVerts[j] = bestTile.data.verts.UnsafeAs<float, Vector3>(bestPoly.verts[j]);
}

var constrainedVerts = constraint.Apply(polyVerts, centerPos, maxRadius);
if (constrainedVerts != null)
{
float polyArea = 0f;

int vertCount = constrainedVerts.Length / 3;
for (int j = 2; j < vertCount; ++j)
{
int va = 0;
int vb = (j - 1) * 3;
int vc = j * 3;
int vb = j - 1;
int vc = j;
polyArea += DtUtils.TriArea2D(constrainedVerts, va, vb, vc);
}

Expand Down Expand Up @@ -405,8 +405,8 @@ public DtStatus FindRandomPointAroundCircle(long startRef, Vector3 centerPos, fl
float s = frand.Next();
float t = frand.Next();

float[] areas = new float[randomPolyVerts.Length / 3];
Vector3 pt = DtUtils.RandomPointInConvexPoly(randomPolyVerts, randomPolyVerts.Length / 3, areas, s, t);
var areas = new float[randomPolyVerts.Length];
var pt = DtUtils.RandomPointInConvexPoly(randomPolyVerts, randomPolyVerts.Length, areas, s, t);
ClosestPointOnPoly(randomPolyRef, pt, out var closest, out _);

randomRef = randomPolyRef;
Expand Down Expand Up @@ -1820,7 +1820,7 @@ public DtStatus MoveAlongSurface(long startRef, Vector3 startPos, Vector3 endPos
var searchPos = Vector3.Lerp(startPos, endPos, 0.5f);
float searchRadSqr = RcMath.Sqr(Vector3.Distance(startPos, endPos) / 2f + 0.001f);

float[] verts = ArrayPool<float>.Shared.Rent(m_nav.GetMaxVertsPerPoly() * 3);
var verts = ArrayPool<Vector3>.Shared.Rent(m_nav.GetMaxVertsPerPoly());

while (0 < stack.Count)
{
Expand All @@ -1837,7 +1837,7 @@ public DtStatus MoveAlongSurface(long startRef, Vector3 startPos, Vector3 endPos
int nverts = curPoly.vertCount;
for (int i = 0; i < nverts; ++i)
{
verts.UnsafeAs<float, Vector3>(i) = curTile.data.verts.UnsafeAs<float, Vector3>(curPoly.verts[i]);
verts[i] = curTile.data.verts.UnsafeAs<float, Vector3>(curPoly.verts[i]);
}

// If target is inside the poly, stop search.
Expand Down Expand Up @@ -1891,14 +1891,14 @@ public DtStatus MoveAlongSurface(long startRef, Vector3 startPos, Vector3 endPos

if (nneis is 0)
{
var vj = verts[j];
var vi = verts[i];
// Wall edge, calc distance.
int vj = j * 3;
int vi = i * 3;
var distSqr = DtUtils.DistancePtSegSqr2D(endPos, verts, vj, vi, out var tseg);
var distSqr = DtUtils.DistancePtSegSqr2D(endPos, vj, vi, out var tseg);
if (distSqr < bestDist)
{
// Update nearest distance.
resultPos = Vector3Extensions.Lerp(verts, vj, vi, tseg);
resultPos = Vector3Extensions.Lerp(vj, vi, tseg);
bestDist = distSqr;
bestNode = curNode;
}
Expand All @@ -1916,9 +1916,7 @@ public DtStatus MoveAlongSurface(long startRef, Vector3 startPos, Vector3 endPos

// Skip the link if it is too far from search constraint.
// TODO: Maybe should use GetPortalPoints(), but this one is way faster.
int vj = j * 3;
int vi = i * 3;
var distSqr = DtUtils.DistancePtSegSqr2D(searchPos, verts, vj, vi, out var _);
var distSqr = DtUtils.DistancePtSegSqr2D(searchPos, verts[j], verts[i], out var _);
if (distSqr > searchRadSqr)
{
continue;
Expand All @@ -1933,7 +1931,7 @@ public DtStatus MoveAlongSurface(long startRef, Vector3 startPos, Vector3 endPos
}
}

ArrayPool<float>.Shared.Return(verts);
ArrayPool<Vector3>.Shared.Return(verts);

if (bestNode != null)
{
Expand Down
2 changes: 1 addition & 1 deletion src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ private DtNoOpDtPolygonByCircleConstraint()
{
}

public ReadOnlySpan<float> Apply(ReadOnlySpan<float> polyVerts, Vector3 circleCenter, float radius)
public ReadOnlySpan<Vector3> Apply(ReadOnlySpan<Vector3> polyVerts, Vector3 circleCenter, float radius)
{
return polyVerts;
}
Expand Down
Loading

0 comments on commit f2e45f7

Please sign in to comment.