/// <summary>
/// Reports all edges intersecting specified ray while ISegmentCast.RegisterCollision returns true.
/// Collisions are reported in order of their distance from rayCast.Origin.
/// </summary>
/// <param name="segmentCast">Cast data</param>
/// <param name="open">NativeList for internal use, is cleared before use</param>
public static void CastRay <T>(this Navmesh.Navmesh navmesh, T rayCast, NativeList <IntPtr> open) where T : IRayCast
{
Assert.IsTrue(rayCast.Distance >= 0, "Ray cast distance should be larger than or equal to zero");
Assert.IsTrue(math.abs(1 - math.length(rayCast.Direction)) < .001f, "Ray cast direction should be normalized");
open.Clear();
var h = navmesh.Extent;
var dist = math.min(math.max(h.x, h.y) * 4, rayCast.Distance);
var org = rayCast.Origin;
var dest = org + rayCast.Direction * dist;
if (!navmesh.Contains(dest))
{
var d = dest;
if (!IntersectSegSeg(org, d, -h, new double2(h.x, -h.y), out dest) &&
!IntersectSegSeg(org, d, new double2(h.x, -h.y), h, out dest) &&
!IntersectSegSeg(org, d, h, new double2(-h.x, h.y), out dest))
{
IntersectSegSeg(org, d, new double2(-h.x, h.y), -h, out dest);
}
}
var segmentCast = new RayCastWrapper <T>(dest, rayCast);
navmesh.CastSegment(segmentCast, open);
}
/// <summary>
/// Reports all edges intersecting specified ray while ISegmentCast.RegisterCollision returns true.
/// Collisions are reported in order of their distance from rayCast.Origin.
/// </summary>
/// <param name="segmentCast">Cast data</param>
/// <param name="allocator">Allocator used to create internal buffers</param>
public static void CastRay <T>(this Navmesh.Navmesh navmesh, T rayCast, Allocator allocator) where T : IRayCast
{
var open = new NativeList <IntPtr>(allocator);
navmesh.CastRay(rayCast, open);
open.Dispose();
}
/// <summary>
/// Reports all edges intersecting specified line segment while ISegmentCast.RegisterCollision returns true.
/// Collisions are reported in order of their distance from segmentCast.Origin.
/// </summary>
/// <param name="segmentCast">Cast data</param>
/// <param name="allocator">Allocator used to create internal buffers</param>
public static void CastSegment <T>(this Navmesh.Navmesh navmesh, T segmentCast, Allocator allocator) where T : ISegmentCast
{
var open = new NativeList <IntPtr>(allocator);
navmesh.CastSegment(segmentCast, open);
open.Dispose();
}
/// <summary>
/// Reports all edges intersecting specified disc while IDiscCast.RegisterCollision returns true.
/// A NativeQueue and NativeHashSet will be created with the specified allocator, and disposed before returning.
/// </summary>
/// <param name="discCast">Cast data</param>
/// <param name="allocator">Allocator used to create internal buffers</param>
public static void CastDisc <T>(this Navmesh.Navmesh navmesh, T discCast, Allocator allocator) where T : IDiscCast
{
var open = new NativeList <IntPtr>(allocator);
var closed = new NativeHashSet <int>(32, allocator);
navmesh.CastDisc(discCast, open, closed);
open.Dispose();
closed.Dispose();
}
/// <summary>
/// Reports all edges intersecting disc while IDiscCast.RegisterCollision returns true, this includes fully contained edges.
/// </summary>
/// <param name="discCast">Cast data</param>
/// <param name="open">NativeQueue for internal use, is cleared before use</param>
/// <param name="closed">NativeHashSet for internal use, is cleared before use</param>
public static void CastDisc <T>(this Navmesh.Navmesh navmesh, T discCast, NativeList <IntPtr> open, NativeHashSet <int> closed) where T : IDiscCast
{
var o = discCast.Origin;
var r = discCast.Radius;
var tri = navmesh.FindTriangleContainingPoint(o);
open.Clear();
closed.Clear();
Check(tri);
Check(tri->LNext);
Check(tri->LPrev);
while (open.Length > 0)
{
tri = (Edge *)open[open.Length - 1];
open.Resize(open.Length - 1, NativeArrayOptions.UninitializedMemory);
Check(tri->LNext);
Check(tri->LPrev);
}
void Check(Edge *edge)
{
if (closed.Contains(edge->QuadEdgeId))
{
return;
}
if (IntersectSegDisc(edge->Org->Point, edge->Dest->Point, o, r))
{
open.Add((IntPtr)edge->Sym);
if (edge->Constrained)
{
discCast.RegisterCollision(edge);
}
}
closed.Add(edge->QuadEdgeId);
}
}
/// <summary>
/// Reports all edges intersecting specified line segment while ISegmentCast.RegisterCollision returns true.
/// Collisions are reported in order of their distance from ISegmentCast.Origin.
/// </summary>
/// <param name="segmentCast">Cast data</param>
/// <param name="open">NativeList for internal use, is cleared before use</param>
public static void CastSegment <T>(this Navmesh.Navmesh navmesh, T segmentCast, NativeList <IntPtr> open) where T : ISegmentCast
{
var o = segmentCast.Origin;
var d = segmentCast.Destination;
var tri = navmesh.FindTriangleContainingPoint(o, out var startCollinear);
var goalEdge = navmesh.FindTriangleContainingPoint(d, out var goalCollinear);
if (ReachedGoal(tri) || startCollinear && ReachedGoal(tri->Sym))
{
RegisterCollinearGoal();
return;
}
open.Clear();
if (math.all(o == tri->Org->Point))
{
if (ExpandVertex(tri->Org))
{
RegisterCollinearGoal();
return;
}
}
else if (math.all(o == tri->Dest->Point))
{
if (ExpandVertex(tri->Dest))
{
RegisterCollinearGoal();
return;
}
}
else if (startCollinear)
{
open.Add((IntPtr)tri->LNext);
open.Add((IntPtr)tri->LPrev);
open.Add((IntPtr)tri->Sym->LNext);
open.Add((IntPtr)tri->Sym->LPrev);
}
else
{
open.Add((IntPtr)tri);
open.Add((IntPtr)tri->LNext);
open.Add((IntPtr)tri->LPrev);
}
do
{
var found = false;
for (int i = 0; i < open.Length; i++)
{
var e = (Edge *)open[i];
if (IntersectSegSeg(o, d, e->Org->Point, e->Dest->Point, out var p))
{
if (e->Constrained)
{
if (!segmentCast.RegisterCollision(p, e))
{
return;
}
}
tri = e->Sym;
open.Clear();
if (math.all(p == tri->Org->Point))
{
o = p;
if (ExpandVertex(tri->Org))
{
RegisterCollinearGoal();
return;
}
}
else if (math.all(p == tri->Dest->Point))
{
o = p;
if (ExpandVertex(tri->Dest))
{
RegisterCollinearGoal();
return;
}
}
else
{
open.Add((IntPtr)tri->LNext);
open.Add((IntPtr)tri->LPrev);
}
found = true;
break;
}
}
Assert.IsTrue(found);
} while (!ReachedGoal(tri));
RegisterCollinearGoal();
bool ExpandVertex(Vertex *vertex)
{
var enumerator = vertex->GetEdgeEnumerator();
while (enumerator.MoveNext())
{
if (ReachedGoal(enumerator.Current))
{
return(true);
}
open.Add((IntPtr)enumerator.Current->LNext);
}
return(false);
}
bool ReachedGoal(Edge *edge) => edge->TriangleId == goalEdge->TriangleId || goalCollinear && edge->TriangleId == goalEdge->Sym->TriangleId;
void RegisterCollinearGoal()
{
if (goalCollinear)
{
segmentCast.RegisterCollision(segmentCast.Destination, goalEdge);
}
}
}
