static Edge *TryGetConstraint(Edge *exit, double clearance, double2 corner, bool lhs) { var edge = lhs ? exit->DNext : exit->DPrev->Sym; return(Navmesh.TryGetConstraint(clearance, corner, edge)); }
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)); } }