/// <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);
}
}
}