public PathQueryState Search(float2 start, float2 goal, Navmesh.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(PathQueryState.StartInvalid);
}
if (!navmesh->Contains(goal))
{
return(PathQueryState.GoalInvalid);
}
var startEdge = navmesh->FindTriangleContainingPoint(start);
if (!EndpointValid(start, radius, startEdge))
{
return(PathQueryState.StartInvalid);
}
var startId = startEdge->TriangleId;
var goalEdge = navmesh->FindTriangleContainingPoint(goal);
if (!EndpointValid(goal, radius, goalEdge))
{
return(PathQueryState.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(PathQueryState.PathFound);
}
}
var ob = this;
GetValidGoalEdges(_validGoalEdges);
if (_validGoalEdges.Length == 0)
{
return(PathQueryState.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(PathQueryState.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.Insert(newStep);
}
}
return(PathQueryState.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.Insert(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.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));
}
}
public unsafe PathQueryState FindPath(float2 from, float2 to, float radius, DynamicBuffer <PathSegmentElement> segments, DynamicBuffer <TriangleElement> triangleIds, Navmesh.Navmesh *navmesh, out int cost)
{
triangleIds.Clear();
var result = _astar.Search(from, to, navmesh, _channel, radius, triangleIds, out cost);
if (result == PathQueryState.PathFound)
{
_funnel.GetPath(_channel, from, to, radius, _path);
triangleIds.Reinterpret <int>().AsNativeArray().Sort();
}
for (int i = 0; i < _path.Count; i++)
{
var w = _path.FromFront(i);
Assert.IsTrue(!float.IsNaN(w.To.x) && !float.IsNaN(w.To.y));
segments.Add(new PathSegmentElement
{
Corner = i > 0 ? _path.FromFront(i - 1).Vertex : w.From,
From = w.From,
To = w.To
});
}
_astar.Clear();
_channel.Clear();
_funnel.Clear();
_path.Clear();
return(result);
}
