/// <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, 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, 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);
}
}
}
public AgentState Search(float2 start, float2 goal, Navmesh navmesh, List <Gate> path, float radius, DynamicBuffer <TriangleElement> triangleIds, out int cost)
{
var open = _open;
var closed = _closed;
var steps = _steps;
var diameter = 2 * radius;
var verts = _verts;
var validGoalEdges = _validGoalEdges;
cost = 80;
if (!navmesh.Contains(start))
{
return(AgentState.StartInvalid);
}
if (!navmesh.Contains(goal))
{
return(AgentState.GoalInvalid);
}
var startEdge = navmesh.FindTriangleContainingPoint(start);
if (!EndpointValid(start, radius, startEdge))
{
return(AgentState.StartInvalid);
}
var startId = startEdge->TriangleId;
var goalEdge = navmesh.FindTriangleContainingPoint(goal);
if (!EndpointValid(goal, radius, goalEdge))
{
return(AgentState.GoalInvalid);
}
var goalId = goalEdge->TriangleId;
var foundDisturbance = false;
if (startId == goalId)
{
var sg = goal - start;
var r = TransformRect((start + goal) / 2, new float2(diameter, math.length(sg)), Math.Angle(sg));
verts.Clear();
VerticesInQuad(r, startEdge, verts);
for (int i = 0; i < verts.Length; i++)
{
var v = verts[i];
var p = v.Vertex->Point;
var psg = Math.ProjectLine(start, goal, p) - start;
v.DistFromStart = math.lengthsq(psg);
if (CheckForDisturbance(p, start, goal, out var opp, startEdge, diameter))
{
foundDisturbance = true;
break;
}
v.Opposite = opp;
verts[i] = v;
}
if (!foundDisturbance)
{
verts.Sort <PossibleDisturbance>();
for (int i = 0; i < verts.Length; i++)
{
var v = verts[i];
var p = v.Vertex->Point;
var e = GeometricPredicates.Orient2DFast(start, goal, p) > 0 ? new Gate {
Left = p, Right = v.Opposite
} : new Gate {
Left = v.Opposite, Right = p
};
if (path.Length > 0)
{
var gate = new Gate {
Left = path[path.Length - 1].Left, Right = e.Right
};
path.Add(gate);
DebugDraw(gate.Left, gate.Right, Color.magenta);
}
path.Add(e);
DebugDraw(e.Left, e.Right, Color.magenta);
}
triangleIds.Add(startId);
return(AgentState.PathFound);
}
}
var ob = this;
GetValidGoalEdges(_validGoalEdges);
if (_validGoalEdges.Length == 0)
{
return(AgentState.NoPath);
}
ExpandInitial(startEdge->Sym);
ExpandInitial(startEdge->LNext->Sym);
ExpandInitial(startEdge->LPrev->Sym);
closed.TryAdd(startId);
closed.TryAdd(goalId);
while (open.Count > 0)
{
var step = open.Extract();
var id = step.Id;
if (id == goalId)
{
var e = step.Edge->TriangleId == goalId ? step.Edge : step.Edge->Sym;
while (step.Previous != -1)
{
path.Add(new Gate {
Left = step.Edge->Org->Point, Right = step.Edge->Dest->Point
});
triangleIds.Add(step.Edge->TriangleId);
step = steps[step.Previous];
}
path.Add(new Gate {
Left = step.Edge->Org->Point, Right = step.Edge->Dest->Point
});
triangleIds.Add(step.Edge->TriangleId);
triangleIds.Add(startId);
AddEndpointEdges(goal, e, true);
e = step.Edge->TriangleId == startId ? step.Edge : step.Edge->Sym;
AddEndpointEdges(start, e, false);
path.Reverse();
return(AgentState.PathFound);
}
closed.TryAdd(id);
var next = step.Edge->LNext;
if (!next->Constrained)
{
Expand(next->Sym, next->ClearanceRight);
}
next = step.Edge->LPrev->Sym;
if (!next->Constrained)
{
Expand(next, next->ClearanceLeft);
}
++cost;
void Expand(Edge *edge, float clearance)
{
if (clearance < diameter)
{
return;
}
if (edge->TriangleId == goalId)
{
if (!validGoalEdges.Contains(edge->QuadEdgeId))
{
return;
}
}
else if (closed.Contains(edge->TriangleId))
{
return;
}
var newStep = new Step
(
edge,
steps.Length,
step.G + C(step.ReferencePoint, edge, out var referencePoint),
H(referencePoint, goal),
step.StepId,
referencePoint
);
steps.Add(newStep);
open.InsertOrLowerKey(newStep);
}
}
return(AgentState.NoPath);
void AddEndpointEdges(float2 endpoint, Edge *edge, bool reverse)
{
var q = new Quad
{
A = edge->Org->Point,
B = edge->Dest->Point,
C = Math.GetTangentRight(edge->Dest->Point, endpoint, radius),
D = Math.GetTangentLeft(edge->Org->Point, endpoint, radius)
};
DebugDraw(q, Color.yellow);
var s = endpoint;
var g = Math.ClosestPointOnLineSegment(s, edge->Org->Point, edge->Dest->Point);
verts.Clear();
ob.VerticesInQuad(q, edge, verts);
for (int i = verts.Length - 1; i >= 0; i--)
{
var v = verts[i];
if (v.Vertex == edge->Org || v.Vertex == edge->Dest)
{
verts.RemoveAt(i);
continue;
}
var p = v.Vertex->Point;
v.DistFromStart = math.distancesq(p, s);
CheckForDisturbance(p, s, g, out var opp, edge, diameter);
v.Opposite = opp;
verts[i] = v;
}
if (verts.Length > 1)
{
verts.Sort <PossibleDisturbance>();
}
if (verts.Length > 0)
{
for (int i = verts.Length - 1; i >= 0; i--)
{
var p = verts[i].Vertex->Point;
if (reverse) // goal
{
var gate = Math.CcwFast(s, g, p)
? new Gate {
Left = path[0].Left, Right = p, IsGoalGate = true
}
: new Gate {
Left = p, Right = path[0].Right, IsGoalGate = true
};
DebugDraw(gate.Left, gate.Right, Color.cyan);
path.Insert(0, gate);
}
else
{
var gate = Math.CcwFast(s, g, p)
? new Gate {
Left = p, Right = path.Last().Right
}
: new Gate {
Left = path.Last().Left, Right = p
};
DebugDraw(gate.Left, gate.Right, Color.magenta);
path.Add(gate);
}
}
}
}
void GetValidGoalEdges(List <int> valid)
{
valid.Clear();
ValidEdge(goalEdge);
ValidEdge(goalEdge->LNext);
ValidEdge(goalEdge->LPrev);
void ValidEdge(Edge *e)
{
if (!e->Constrained && !EndpointDisturbed(e, goal))
{
valid.Add(e->QuadEdgeId);
}
}
}
void ExpandInitial(Edge *edge)
{
if (edge->Constrained || edge->TriangleId == goalId && !validGoalEdges.Contains(edge->QuadEdgeId))
{
return;
}
if (EndpointDisturbed(edge->Sym, start))
{
return;
}
var newStep = new Step
(
edge,
steps.Length,
C(start, edge, out var referencePoint),
H(referencePoint, goal),
-1,
referencePoint
);
steps.Add(newStep);
open.InsertOrLowerKey(newStep);
}
bool EndpointDisturbed(Edge *edge, float2 endpoint)
{
var q = new Quad
{
A = edge->Org->Point,
B = edge->Dest->Point,
C = Math.GetTangentRight(edge->Dest->Point, endpoint, radius),
D = Math.GetTangentLeft(edge->Org->Point, endpoint, radius)
};
verts.Clear();
ob.VerticesInQuad(q, edge, verts);
for (int i = 0; i < verts.Length; i++)
{
var v = verts[i];
var p = v.Vertex->Point;
if (CheckForDisturbance(p, endpoint, (edge->Org->Point + edge->Dest->Point) / 2, out _, edge, diameter))
{
return(true);
}
}
return(false);
}
float C(float2 from, Edge *edge, out float2 referencePoint)
{
var o = edge->Org->Point;
var d = edge->Dest->Point;
var od = d - o;
var offset = math.normalize(od) * radius;
o += offset;
d -= offset;
if (!Math.IntersectSegSeg(from, goal, o, d, out referencePoint))
{
// metric 4 from paper
// referencePoint = math.lengthsq(goal - o) < math.lengthsq(goal - d) ? o : d;
referencePoint = Math.ClosestPointOnLineSegment(from, o, d);
}
return(math.length(referencePoint - from));
}
float H(float2 p0, float2 p1)
{
return(math.length(p1 - p0));
}
bool CheckForDisturbance(float2 v, float2 s, float2 g, out float2 opposite, Edge *tri, float d)
{
Edge *e;
if (Math.Ccw(tri->Org->Point, tri->Dest->Point, v) && Math.ProjectSeg(tri->Org->Point, tri->Dest->Point, v, out _))
{
e = tri;
}
else if (Math.Ccw(tri->LNext->Org->Point, tri->LNext->Dest->Point, v) && Math.ProjectSeg(tri->LNext->Org->Point, tri->LNext->Dest->Point, v, out _))
{
e = tri->LNext;
}
else if (Math.Ccw(tri->LPrev->Org->Point, tri->LPrev->Dest->Point, v) && Math.ProjectSeg(tri->LPrev->Org->Point, tri->LPrev->Dest->Point, v, out _))
{
e = tri->LPrev;
}
else
{
if (math.any(tri->Org->Point != v) && math.any(tri->Dest->Point != v))
{
e = tri;
}
else if (math.any(tri->LNext->Org->Point != v) && math.any(tri->LNext->Dest->Point != v))
{
e = tri->LNext;
}
else if (math.any(tri->LPrev->Org->Point != v) && math.any(tri->LPrev->Dest->Point != v))
{
e = tri->LPrev;
}
else
{
throw new Exception();
}
}
var sgi = Math.ProjectLine(e->Org->Point, e->Dest->Point, v);
opposite = (float2)(sgi + math.normalize(sgi - v) * d);
var c = Navmesh.TryGetConstraint(math.length(opposite - v), v, e->Sym);
if (c == null)
{
return(false);
}
opposite = (float2)Math.ProjectLine(c->Org->Point, c->Dest->Point, v);
return(math.length(opposite - v) < d && Math.IntersectSegSeg(v, opposite, s, g));
}
}
