800 lines
26 KiB
C#
800 lines
26 KiB
C#
using UnityEngine;
|
|
using System.Collections.Generic;
|
|
using Pathfinding.Util;
|
|
using Pathfinding.Pooling;
|
|
using UnityEngine.Assertions;
|
|
|
|
namespace Pathfinding {
|
|
public class RichPath {
|
|
int currentPart;
|
|
readonly List<RichPathPart> parts = new List<RichPathPart>();
|
|
|
|
public Seeker seeker;
|
|
|
|
/// <summary>
|
|
/// Transforms points from path space to world space.
|
|
/// If null the identity transform will be used.
|
|
///
|
|
/// This is used when the world position of the agent does not match the
|
|
/// corresponding position on the graph. This is the case in the example
|
|
/// scene called 'Moving'.
|
|
///
|
|
/// See: <see cref="Pathfinding.Examples.LocalSpaceRichAI"/>
|
|
/// </summary>
|
|
public ITransform transform;
|
|
|
|
public RichPath () {
|
|
Clear();
|
|
}
|
|
|
|
public void Clear () {
|
|
parts.Clear();
|
|
currentPart = 0;
|
|
Endpoint = new Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
|
|
}
|
|
|
|
/// <summary>Use this for initialization.</summary>
|
|
/// <param name="seeker">Optionally provide in order to take tag penalties into account. May be null if you do not use a Seeker\</param>
|
|
/// <param name="path">Path to follow</param>
|
|
/// <param name="mergePartEndpoints">If true, then adjacent parts that the path is split up in will
|
|
/// try to use the same start/end points. For example when using a link on a navmesh graph
|
|
/// Instead of first following the path to the center of the node where the link is and then
|
|
/// follow the link, the path will be adjusted to go to the exact point where the link starts
|
|
/// which usually makes more sense.</param>
|
|
/// <param name="simplificationMode">The path can optionally be simplified. This can be a bit expensive for long paths.</param>
|
|
public void Initialize (Seeker seeker, Path path, bool mergePartEndpoints, bool simplificationMode) {
|
|
if (path.error) throw new System.ArgumentException("Path has an error");
|
|
|
|
List<GraphNode> nodes = path.path;
|
|
if (nodes.Count == 0) throw new System.ArgumentException("Path traverses no nodes");
|
|
|
|
this.seeker = seeker;
|
|
// Release objects back to object pool
|
|
// Yeah, I know, it's casting... but this won't be called much
|
|
for (int i = 0; i < parts.Count; i++) {
|
|
var funnelPart = parts[i] as RichFunnel;
|
|
var specialPart = parts[i] as RichSpecial;
|
|
if (funnelPart != null) ObjectPool<RichFunnel>.Release(ref funnelPart);
|
|
else if (specialPart != null) ObjectPool<RichSpecial>.Release(ref specialPart);
|
|
}
|
|
|
|
Clear();
|
|
|
|
// Initialize new
|
|
Endpoint = path.vectorPath[path.vectorPath.Count-1];
|
|
|
|
//Break path into parts
|
|
for (int i = 0; i < nodes.Count; i++) {
|
|
if (nodes[i] is TriangleMeshNode) {
|
|
var graph = AstarData.GetGraph(nodes[i]) as NavmeshBase;
|
|
if (graph == null) throw new System.Exception("Found a TriangleMeshNode that was not in a NavmeshBase graph");
|
|
|
|
RichFunnel f = ObjectPool<RichFunnel>.Claim().Initialize(this, graph);
|
|
|
|
f.funnelSimplification = simplificationMode;
|
|
|
|
int sIndex = i;
|
|
uint currentGraphIndex = nodes[sIndex].GraphIndex;
|
|
|
|
|
|
for (; i < nodes.Count; i++) {
|
|
if (nodes[i].GraphIndex != currentGraphIndex && !(nodes[i] is NodeLink3Node)) {
|
|
break;
|
|
}
|
|
}
|
|
i--;
|
|
|
|
if (sIndex == 0) {
|
|
f.exactStart = path.vectorPath[0];
|
|
} else {
|
|
f.exactStart = (Vector3)nodes[mergePartEndpoints ? sIndex-1 : sIndex].position;
|
|
}
|
|
|
|
if (i == nodes.Count-1) {
|
|
f.exactEnd = path.vectorPath[path.vectorPath.Count-1];
|
|
} else {
|
|
f.exactEnd = (Vector3)nodes[mergePartEndpoints ? i+1 : i].position;
|
|
}
|
|
|
|
f.BuildFunnelCorridor(nodes, sIndex, i);
|
|
|
|
parts.Add(f);
|
|
} else if (nodes[i] is LinkNode nl) {
|
|
int sIndex = i;
|
|
uint currentGraphIndex = nodes[sIndex].GraphIndex;
|
|
|
|
while (i < nodes.Count && nodes[i].GraphIndex == currentGraphIndex) i++;
|
|
i--;
|
|
|
|
if (i - sIndex > 1) {
|
|
throw new System.Exception("NodeLink2 path length greater than two (2) nodes. " + (i - sIndex));
|
|
} else if (i - sIndex == 0) {
|
|
// The link is a single node.
|
|
// Just ignore it. It can happen in very rare circumstances with some path types.
|
|
// For example, a RandomPath can stop at the first node of a node link, without including the other end of the link
|
|
continue;
|
|
}
|
|
Assert.AreEqual(nl.linkConcrete, (nodes[i] as LinkNode).linkConcrete);
|
|
|
|
RichSpecial rps = ObjectPool<RichSpecial>.Claim().Initialize(nl.linkConcrete.GetTracer(nl));
|
|
parts.Add(rps);
|
|
} else if (!(nodes[i] is PointNode)) {
|
|
// Some other graph type which we do not have support for
|
|
throw new System.InvalidOperationException("The RichAI movment script can only be used on recast/navmesh graphs. A node of type " + nodes[i].GetType().Name + " was in the path.");
|
|
}
|
|
}
|
|
}
|
|
|
|
public Vector3 Endpoint { get; private set; }
|
|
|
|
/// <summary>True if we have completed (called NextPart for) the last part in the path</summary>
|
|
public bool CompletedAllParts {
|
|
get {
|
|
return currentPart >= parts.Count;
|
|
}
|
|
}
|
|
|
|
/// <summary>True if we are traversing the last part of the path</summary>
|
|
public bool IsLastPart {
|
|
get {
|
|
return currentPart >= parts.Count - 1;
|
|
}
|
|
}
|
|
|
|
public void NextPart () {
|
|
currentPart = Mathf.Min(currentPart + 1, parts.Count);
|
|
}
|
|
|
|
public RichPathPart GetCurrentPart () {
|
|
if (parts.Count == 0) return null;
|
|
return currentPart < parts.Count ? parts[currentPart] : parts[parts.Count - 1];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Replaces the buffer with the remaining path.
|
|
/// See: <see cref="Pathfinding.IAstarAI.GetRemainingPath"/>
|
|
/// </summary>
|
|
public void GetRemainingPath (List<Vector3> buffer, List<PathPartWithLinkInfo> partsBuffer, Vector3 currentPosition, out bool requiresRepath) {
|
|
buffer.Clear();
|
|
buffer.Add(currentPosition);
|
|
requiresRepath = false;
|
|
for (int i = currentPart; i < parts.Count; i++) {
|
|
var part = parts[i];
|
|
if (part is RichFunnel funnel) {
|
|
bool lastCorner;
|
|
var startIndex = buffer.Count;
|
|
if (i != 0) buffer.Add(funnel.exactStart);
|
|
funnel.Update(i == 0 ? currentPosition : funnel.exactStart, buffer, int.MaxValue, out lastCorner, out requiresRepath);
|
|
if (partsBuffer != null) partsBuffer.Add(new PathPartWithLinkInfo(startIndex, buffer.Count-1));
|
|
if (requiresRepath) {
|
|
return;
|
|
}
|
|
} else if (part is RichSpecial rs) {
|
|
// By adding all points above the link will look like just a stright line, which is reasonable
|
|
// The part's start/end indices refer to the last point in previous part and first point in the next part, respectively
|
|
if (partsBuffer != null) partsBuffer.Add(new PathPartWithLinkInfo(buffer.Count-1, buffer.Count, rs.nodeLink));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public abstract class RichPathPart : IAstarPooledObject {
|
|
public abstract void OnEnterPool();
|
|
}
|
|
|
|
public class RichFunnel : RichPathPart {
|
|
readonly List<Vector3> left;
|
|
readonly List<Vector3> right;
|
|
List<TriangleMeshNode> nodes;
|
|
public Vector3 exactStart;
|
|
public Vector3 exactEnd;
|
|
NavmeshBase graph;
|
|
int currentNode;
|
|
Vector3 currentPosition;
|
|
int checkForDestroyedNodesCounter;
|
|
RichPath path;
|
|
int[] triBuffer = new int[3];
|
|
|
|
/// <summary>Post process the funnel corridor or not</summary>
|
|
public bool funnelSimplification = true;
|
|
|
|
public RichFunnel () {
|
|
left = Pathfinding.Pooling.ListPool<Vector3>.Claim();
|
|
right = Pathfinding.Pooling.ListPool<Vector3>.Claim();
|
|
nodes = new List<TriangleMeshNode>();
|
|
this.graph = null;
|
|
}
|
|
|
|
/// <summary>Works like a constructor, but can be used even for pooled objects. Returns this for easy chaining</summary>
|
|
public RichFunnel Initialize (RichPath path, NavmeshBase graph) {
|
|
if (graph == null) throw new System.ArgumentNullException("graph");
|
|
if (this.graph != null) throw new System.InvalidOperationException("Trying to initialize an already initialized object. " + graph);
|
|
|
|
this.graph = graph;
|
|
this.path = path;
|
|
return this;
|
|
}
|
|
|
|
public override void OnEnterPool () {
|
|
left.Clear();
|
|
right.Clear();
|
|
nodes.Clear();
|
|
graph = null;
|
|
currentNode = 0;
|
|
checkForDestroyedNodesCounter = 0;
|
|
}
|
|
|
|
public TriangleMeshNode CurrentNode {
|
|
get {
|
|
var node = nodes[currentNode];
|
|
if (!node.Destroyed) {
|
|
return node;
|
|
}
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Build a funnel corridor from a node list slice.
|
|
/// The nodes are assumed to be of type TriangleMeshNode.
|
|
/// </summary>
|
|
/// <param name="nodes">Nodes to build the funnel corridor from</param>
|
|
/// <param name="start">Start index in the nodes list</param>
|
|
/// <param name="end">End index in the nodes list, this index is inclusive</param>
|
|
public void BuildFunnelCorridor (List<GraphNode> nodes, int start, int end) {
|
|
//Make sure start and end points are on the correct nodes
|
|
exactStart = (nodes[start] as MeshNode).ClosestPointOnNode(exactStart);
|
|
exactEnd = (nodes[end] as MeshNode).ClosestPointOnNode(exactEnd);
|
|
|
|
left.Clear();
|
|
right.Clear();
|
|
left.Add(exactStart);
|
|
right.Add(exactStart);
|
|
|
|
this.nodes.Clear();
|
|
|
|
if (funnelSimplification) {
|
|
List<GraphNode> tmp = ListPool<GraphNode>.Claim(end-start);
|
|
|
|
var tagPenalties = path.seeker != null ? path.seeker.tagPenalties : Path.ZeroTagPenalties;
|
|
var traversableTags = path.seeker != null ? path.seeker.traversableTags : -1;
|
|
|
|
Funnel.Simplify(new Funnel.PathPart {
|
|
startIndex = start,
|
|
endIndex = end,
|
|
startPoint = exactStart,
|
|
endPoint = exactEnd,
|
|
type = Funnel.PartType.NodeSequence,
|
|
}, graph, nodes, tmp, tagPenalties, traversableTags);
|
|
|
|
if (this.nodes.Capacity < tmp.Count) this.nodes.Capacity = tmp.Count;
|
|
|
|
for (int i = 0; i < tmp.Count; i++) {
|
|
// Guaranteed to be TriangleMeshNodes since they are all in the same graph
|
|
var node = tmp[i] as TriangleMeshNode;
|
|
if (node != null) this.nodes.Add(node);
|
|
}
|
|
|
|
ListPool<GraphNode>.Release(ref tmp);
|
|
} else {
|
|
if (this.nodes.Capacity < end-start) this.nodes.Capacity = (end-start);
|
|
for (int i = start; i <= end; i++) {
|
|
// Guaranteed to be TriangleMeshNodes since they are all in the same graph
|
|
var node = nodes[i] as TriangleMeshNode;
|
|
if (node != null) this.nodes.Add(node);
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < this.nodes.Count-1; i++) {
|
|
if (this.nodes[i].GetPortal(this.nodes[i+1], out var leftP, out var rightP)) {
|
|
left.Add(leftP);
|
|
right.Add(rightP);
|
|
} else {
|
|
// Can happen in case custom connections have been added
|
|
left.Add((Vector3)this.nodes[i].position);
|
|
right.Add((Vector3)this.nodes[i].position);
|
|
left.Add((Vector3)this.nodes[i+1].position);
|
|
right.Add((Vector3)this.nodes[i+1].position);
|
|
}
|
|
}
|
|
|
|
left.Add(exactEnd);
|
|
right.Add(exactEnd);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Split funnel at node index splitIndex and throw the nodes up to that point away and replace with prefix.
|
|
/// Used when the AI has happened to get sidetracked and entered a node outside the funnel.
|
|
/// </summary>
|
|
void UpdateFunnelCorridor (int splitIndex, List<TriangleMeshNode> prefix) {
|
|
nodes.RemoveRange(0, splitIndex);
|
|
nodes.InsertRange(0, prefix);
|
|
|
|
left.Clear();
|
|
right.Clear();
|
|
left.Add(exactStart);
|
|
right.Add(exactStart);
|
|
|
|
for (int i = 0; i < nodes.Count-1; i++) {
|
|
if (nodes[i].GetPortal(nodes[i+1], out var leftP, out var rightP)) {
|
|
left.Add(leftP);
|
|
right.Add(rightP);
|
|
}
|
|
}
|
|
|
|
left.Add(exactEnd);
|
|
right.Add(exactEnd);
|
|
}
|
|
|
|
/// <summary>True if any node in the path is destroyed</summary>
|
|
bool CheckForDestroyedNodes () {
|
|
// Loop through all nodes and check if they are destroyed
|
|
// If so, we really need a recalculation of our path quickly
|
|
// since there might be an obstacle blocking our path after
|
|
// a graph update or something similar
|
|
for (int i = 0, t = nodes.Count; i < t; i++) {
|
|
if (nodes[i].Destroyed) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Approximate distance (as the crow flies) to the endpoint of this path part.
|
|
/// See: <see cref="exactEnd"/>
|
|
/// </summary>
|
|
public float DistanceToEndOfPath {
|
|
get {
|
|
var currentNode = CurrentNode;
|
|
Vector3 closestOnNode = currentNode != null? currentNode.ClosestPointOnNode(currentPosition) : currentPosition;
|
|
return (exactEnd - closestOnNode).magnitude;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clamps the position to the navmesh and repairs the path if the agent has moved slightly outside it.
|
|
/// You should not call this method with anything other than the agent's position.
|
|
/// </summary>
|
|
public Vector3 ClampToNavmesh (Vector3 position) {
|
|
if (path.transform != null) position = path.transform.InverseTransform(position);
|
|
UnityEngine.Assertions.Assert.IsFalse(float.IsNaN(position.x));
|
|
ClampToNavmeshInternal(ref position);
|
|
if (path.transform != null) position = path.transform.Transform(position);
|
|
UnityEngine.Assertions.Assert.IsFalse(float.IsNaN(position.x));
|
|
return position;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find the next points to move towards and clamp the position to the navmesh.
|
|
///
|
|
/// Returns: The position of the agent clamped to make sure it is inside the navmesh.
|
|
/// </summary>
|
|
/// <param name="position">The position of the agent.</param>
|
|
/// <param name="buffer">Will be filled with up to numCorners points which are the next points in the path towards the target.</param>
|
|
/// <param name="numCorners">See buffer.</param>
|
|
/// <param name="lastCorner">True if the buffer contains the end point of the path.</param>
|
|
/// <param name="requiresRepath">True if nodes along the path have been destroyed and a path recalculation is necessary.</param>
|
|
public Vector3 Update (Vector3 position, List<Vector3> buffer, int numCorners, out bool lastCorner, out bool requiresRepath) {
|
|
if (path.transform != null) position = path.transform.InverseTransform(position);
|
|
UnityEngine.Assertions.Assert.IsFalse(float.IsNaN(position.x));
|
|
|
|
lastCorner = false;
|
|
requiresRepath = false;
|
|
|
|
// Only check for destroyed nodes every 10 frames
|
|
if (checkForDestroyedNodesCounter >= 10) {
|
|
checkForDestroyedNodesCounter = 0;
|
|
requiresRepath |= CheckForDestroyedNodes();
|
|
} else {
|
|
checkForDestroyedNodesCounter++;
|
|
}
|
|
|
|
bool nodesDestroyed = ClampToNavmeshInternal(ref position);
|
|
|
|
currentPosition = position;
|
|
|
|
if (nodesDestroyed) {
|
|
// Some nodes on the path have been destroyed
|
|
// we need to recalculate the path immediately
|
|
requiresRepath = true;
|
|
lastCorner = false;
|
|
buffer.Add(position);
|
|
} else if (!FindNextCorners(position, currentNode, buffer, numCorners, out lastCorner)) {
|
|
Debug.LogError("Failed to find next corners in the path");
|
|
buffer.Add(position);
|
|
}
|
|
|
|
if (path.transform != null) {
|
|
for (int i = 0; i < buffer.Count; i++) {
|
|
buffer[i] = path.transform.Transform(buffer[i]);
|
|
}
|
|
|
|
position = path.transform.Transform(position);
|
|
UnityEngine.Assertions.Assert.IsFalse(float.IsNaN(position.x));
|
|
}
|
|
|
|
return position;
|
|
}
|
|
|
|
/// <summary>Cached object to avoid unnecessary allocations</summary>
|
|
static Queue<TriangleMeshNode> navmeshClampQueue = new Queue<TriangleMeshNode>();
|
|
/// <summary>Cached object to avoid unnecessary allocations</summary>
|
|
static List<TriangleMeshNode> navmeshClampList = new List<TriangleMeshNode>();
|
|
/// <summary>Cached object to avoid unnecessary allocations</summary>
|
|
static Dictionary<TriangleMeshNode, TriangleMeshNode> navmeshClampDict = new Dictionary<TriangleMeshNode, TriangleMeshNode>();
|
|
|
|
/// <summary>
|
|
/// Searches for the node the agent is inside.
|
|
/// This will also clamp the position to the navmesh
|
|
/// and repair the funnel cooridor if the agent moves slightly outside it.
|
|
///
|
|
/// Returns: True if nodes along the path have been destroyed so that a path recalculation is required
|
|
/// </summary>
|
|
bool ClampToNavmeshInternal (ref Vector3 position) {
|
|
var previousNode = nodes[currentNode];
|
|
|
|
if (previousNode.Destroyed) {
|
|
return true;
|
|
}
|
|
|
|
// Check if we are in the same node as we were in during the last frame and otherwise do a more extensive search
|
|
if (previousNode.ContainsPoint(position)) {
|
|
return false;
|
|
}
|
|
|
|
// This part of the code is relatively seldom called
|
|
// Most of the time we are still on the same node as during the previous frame
|
|
|
|
var que = navmeshClampQueue;
|
|
var allVisited = navmeshClampList;
|
|
var parent = navmeshClampDict;
|
|
previousNode.TemporaryFlag1 = true;
|
|
parent[previousNode] = null;
|
|
que.Enqueue(previousNode);
|
|
allVisited.Add(previousNode);
|
|
|
|
float bestDistance = float.PositiveInfinity;
|
|
Vector3 bestPoint = position;
|
|
TriangleMeshNode bestNode = null;
|
|
|
|
while (que.Count > 0) {
|
|
var node = que.Dequeue();
|
|
|
|
// Snap to the closest point in XZ space (keep the Y coordinate)
|
|
// If we would have snapped to the closest point in 3D space, the agent
|
|
// might slow down when traversing slopes
|
|
var closest = node.ClosestPointOnNodeXZ(position);
|
|
var dist = VectorMath.MagnitudeXZ(closest - position);
|
|
|
|
// Check if this node is any closer than the previous best node.
|
|
// Allow for a small margin to both avoid floating point errors and to allow
|
|
// moving past very small local minima.
|
|
if (dist <= bestDistance * 1.05f + 0.001f) {
|
|
if (dist < bestDistance) {
|
|
bestDistance = dist;
|
|
bestPoint = closest;
|
|
bestNode = node;
|
|
}
|
|
|
|
for (int i = 0; i < node.connections.Length; i++) {
|
|
if (!node.connections[i].isOutgoing) continue;
|
|
var neighbour = node.connections[i].node as TriangleMeshNode;
|
|
if (neighbour != null && !neighbour.TemporaryFlag1) {
|
|
neighbour.TemporaryFlag1 = true;
|
|
parent[neighbour] = node;
|
|
que.Enqueue(neighbour);
|
|
allVisited.Add(neighbour);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
UnityEngine.Assertions.Assert.IsNotNull(bestNode);
|
|
|
|
for (int i = 0; i < allVisited.Count; i++) allVisited[i].TemporaryFlag1 = false;
|
|
allVisited.ClearFast();
|
|
|
|
var closestNodeInPath = nodes.IndexOf(bestNode);
|
|
|
|
// Move the x and z coordinates of the chararacter but not the y coordinate
|
|
// because the navmesh surface may not line up with the ground
|
|
position.x = bestPoint.x;
|
|
position.z = bestPoint.z;
|
|
|
|
// Check if the closest node
|
|
// was on the path already or if we need to adjust it
|
|
if (closestNodeInPath == -1) {
|
|
// Reuse this list, because why not.
|
|
var prefix = navmeshClampList;
|
|
|
|
while (closestNodeInPath == -1) {
|
|
prefix.Add(bestNode);
|
|
bestNode = parent[bestNode];
|
|
closestNodeInPath = nodes.IndexOf(bestNode);
|
|
}
|
|
|
|
// We have found a node containing the position, but it is outside the funnel
|
|
// Recalculate the funnel to include this node
|
|
exactStart = position;
|
|
UpdateFunnelCorridor(closestNodeInPath, prefix);
|
|
|
|
prefix.ClearFast();
|
|
|
|
// Restart from the first node in the updated path
|
|
currentNode = 0;
|
|
} else {
|
|
currentNode = closestNodeInPath;
|
|
}
|
|
|
|
parent.Clear();
|
|
// Do a quick check to see if the next node in the path has been destroyed
|
|
// If that is the case then we should plan a new path immediately
|
|
return currentNode + 1 < nodes.Count && nodes[currentNode+1].Destroyed;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Fill wallBuffer with all navmesh wall segments close to the current position.
|
|
/// A wall segment is a node edge which is not shared by any other neighbour node, i.e an outer edge on the navmesh.
|
|
/// </summary>
|
|
public void FindWalls (List<Vector3> wallBuffer, float range) {
|
|
FindWalls(currentNode, wallBuffer, currentPosition, range);
|
|
}
|
|
|
|
void FindWalls (int nodeIndex, List<Vector3> wallBuffer, Vector3 position, float range) {
|
|
if (range <= 0) return;
|
|
|
|
bool negAbort = false;
|
|
bool posAbort = false;
|
|
|
|
range *= range;
|
|
|
|
//Looping as 0,-1,1,-2,2,-3,3,-4,4 etc. Avoids code duplication by keeping it to one loop instead of two
|
|
for (int i = 0; !negAbort || !posAbort; i = i < 0 ? -i : -i-1) {
|
|
if (i < 0 && negAbort) continue;
|
|
if (i > 0 && posAbort) continue;
|
|
|
|
if (i < 0 && nodeIndex+i < 0) {
|
|
negAbort = true;
|
|
continue;
|
|
}
|
|
|
|
if (i > 0 && nodeIndex+i >= nodes.Count) {
|
|
posAbort = true;
|
|
continue;
|
|
}
|
|
|
|
TriangleMeshNode prev = nodeIndex+i-1 < 0 ? null : nodes[nodeIndex+i-1];
|
|
TriangleMeshNode node = nodes[nodeIndex+i];
|
|
TriangleMeshNode next = nodeIndex+i+1 >= nodes.Count ? null : nodes[nodeIndex+i+1];
|
|
|
|
if (node.Destroyed) {
|
|
break;
|
|
}
|
|
|
|
var dir = node.ClosestPointOnNodeXZ(position)-position;
|
|
dir.y = 0;
|
|
if (dir.sqrMagnitude > range) {
|
|
if (i < 0) negAbort = true;
|
|
else posAbort = true;
|
|
continue;
|
|
}
|
|
|
|
for (int j = 0; j < 3; j++) triBuffer[j] = 0;
|
|
|
|
for (int j = 0; j < node.connections.Length; j++) {
|
|
var other = node.connections[j].node as TriangleMeshNode;
|
|
if (other == null) continue;
|
|
|
|
int va = -1;
|
|
for (int a = 0; a < 3; a++) {
|
|
for (int b = 0; b < 3; b++) {
|
|
if (node.GetVertex(a) == other.GetVertex((b+1) % 3) && node.GetVertex((a+1) % 3) == other.GetVertex(b)) {
|
|
va = a;
|
|
a = 3;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if (va == -1) {
|
|
//No direct connection
|
|
} else {
|
|
triBuffer[va] = other == prev || other == next ? 2 : 1;
|
|
}
|
|
}
|
|
|
|
for (int j = 0; j < 3; j++) {
|
|
//Tribuffer values
|
|
// 0 : Navmesh border, outer edge
|
|
// 1 : Inner edge, to node inside funnel
|
|
// 2 : Inner edge, to node outside funnel
|
|
if (triBuffer[j] == 0) {
|
|
//Add edge to list of walls
|
|
wallBuffer.Add((Vector3)node.GetVertex(j));
|
|
wallBuffer.Add((Vector3)node.GetVertex((j+1) % 3));
|
|
}
|
|
}
|
|
}
|
|
|
|
if (path.transform != null) {
|
|
for (int i = 0; i < wallBuffer.Count; i++) {
|
|
wallBuffer[i] = path.transform.Transform(wallBuffer[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
bool FindNextCorners (Vector3 origin, int startIndex, List<Vector3> funnelPath, int numCorners, out bool lastCorner) {
|
|
lastCorner = false;
|
|
|
|
if (left == null) throw new System.Exception("left list is null");
|
|
if (right == null) throw new System.Exception("right list is null");
|
|
if (funnelPath == null) throw new System.ArgumentNullException("funnelPath");
|
|
|
|
if (left.Count != right.Count) throw new System.ArgumentException("left and right lists must have equal length");
|
|
|
|
int diagonalCount = left.Count;
|
|
|
|
if (diagonalCount == 0) throw new System.ArgumentException("no diagonals");
|
|
|
|
if (diagonalCount-startIndex < 3) {
|
|
//Direct path
|
|
funnelPath.Add(left[diagonalCount-1]);
|
|
lastCorner = true;
|
|
return true;
|
|
}
|
|
|
|
#if ASTARDEBUG
|
|
for (int i = startIndex; i < left.Count-1; i++) {
|
|
Debug.DrawLine(left[i], left[i+1], Color.red);
|
|
Debug.DrawLine(right[i], right[i+1], Color.magenta);
|
|
Debug.DrawRay(right[i], Vector3.up, Color.magenta);
|
|
}
|
|
for (int i = 0; i < left.Count; i++) {
|
|
Debug.DrawLine(right[i], left[i], Color.cyan);
|
|
}
|
|
#endif
|
|
|
|
//Remove identical vertices
|
|
while (left[startIndex+1] == left[startIndex+2] && right[startIndex+1] == right[startIndex+2]) {
|
|
//System.Console.WriteLine ("Removing identical left and right");
|
|
//left.RemoveAt (1);
|
|
//right.RemoveAt (1);
|
|
startIndex++;
|
|
|
|
if (diagonalCount-startIndex <= 3) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
Vector3 swPoint = left[startIndex+2];
|
|
if (swPoint == left[startIndex+1]) {
|
|
swPoint = right[startIndex+2];
|
|
}
|
|
|
|
|
|
//Test
|
|
while (VectorMath.IsColinearXZ(origin, left[startIndex+1], right[startIndex+1]) || VectorMath.RightOrColinearXZ(left[startIndex+1], right[startIndex+1], swPoint) == VectorMath.RightOrColinearXZ(left[startIndex+1], right[startIndex+1], origin)) {
|
|
#if ASTARDEBUG
|
|
Debug.DrawLine(left[startIndex+1], right[startIndex+1], new Color(0, 0, 0, 0.5F));
|
|
Debug.DrawLine(origin, swPoint, new Color(0, 0, 0, 0.5F));
|
|
#endif
|
|
//left.RemoveAt (1);
|
|
//right.RemoveAt (1);
|
|
startIndex++;
|
|
|
|
if (diagonalCount-startIndex < 3) {
|
|
//Debug.Log ("#2 " + left.Count + " - " + startIndex + " = " + (left.Count-startIndex));
|
|
//Direct path
|
|
funnelPath.Add(left[diagonalCount-1]);
|
|
lastCorner = true;
|
|
return true;
|
|
}
|
|
|
|
swPoint = left[startIndex+2];
|
|
if (swPoint == left[startIndex+1]) {
|
|
swPoint = right[startIndex+2];
|
|
}
|
|
}
|
|
|
|
|
|
//funnelPath.Add (origin);
|
|
|
|
Vector3 portalApex = origin;
|
|
Vector3 portalLeft = left[startIndex+1];
|
|
Vector3 portalRight = right[startIndex+1];
|
|
|
|
int apexIndex = startIndex+0;
|
|
int rightIndex = startIndex+1;
|
|
int leftIndex = startIndex+1;
|
|
|
|
for (int i = startIndex+2; i < diagonalCount; i++) {
|
|
if (funnelPath.Count >= numCorners) {
|
|
return true;
|
|
}
|
|
|
|
if (funnelPath.Count > 2000) {
|
|
Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
|
|
break;
|
|
}
|
|
|
|
Vector3 pLeft = left[i];
|
|
Vector3 pRight = right[i];
|
|
|
|
/*Debug.DrawLine (portalApex,portalLeft,Color.red);
|
|
* Debug.DrawLine (portalApex,portalRight,Color.yellow);
|
|
* Debug.DrawLine (portalApex,left,Color.cyan);
|
|
* Debug.DrawLine (portalApex,right,Color.cyan);*/
|
|
|
|
if (VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalRight, pRight) >= 0) {
|
|
if (portalApex == portalRight || VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalLeft, pRight) <= 0) {
|
|
portalRight = pRight;
|
|
rightIndex = i;
|
|
} else {
|
|
funnelPath.Add(portalLeft);
|
|
portalApex = portalLeft;
|
|
apexIndex = leftIndex;
|
|
|
|
portalLeft = portalApex;
|
|
portalRight = portalApex;
|
|
|
|
leftIndex = apexIndex;
|
|
rightIndex = apexIndex;
|
|
|
|
i = apexIndex;
|
|
|
|
continue;
|
|
}
|
|
}
|
|
|
|
if (VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalLeft, pLeft) <= 0) {
|
|
if (portalApex == portalLeft || VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalRight, pLeft) >= 0) {
|
|
portalLeft = pLeft;
|
|
leftIndex = i;
|
|
} else {
|
|
funnelPath.Add(portalRight);
|
|
portalApex = portalRight;
|
|
apexIndex = rightIndex;
|
|
|
|
portalLeft = portalApex;
|
|
portalRight = portalApex;
|
|
|
|
leftIndex = apexIndex;
|
|
rightIndex = apexIndex;
|
|
|
|
i = apexIndex;
|
|
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
|
|
lastCorner = true;
|
|
funnelPath.Add(left[diagonalCount-1]);
|
|
|
|
return true;
|
|
}
|
|
}
|
|
|
|
public struct FakeTransform {
|
|
public Vector3 position;
|
|
public Quaternion rotation;
|
|
}
|
|
|
|
public class RichSpecial : RichPathPart {
|
|
public OffMeshLinks.OffMeshLinkTracer nodeLink;
|
|
public FakeTransform first => new FakeTransform { position = nodeLink.relativeStart, rotation = nodeLink.isReverse ? nodeLink.link.end.rotation : nodeLink.link.start.rotation };
|
|
public FakeTransform second => new FakeTransform { position = nodeLink.relativeEnd, rotation = nodeLink.isReverse ? nodeLink.link.start.rotation : nodeLink.link.end.rotation };
|
|
public bool reverse => nodeLink.isReverse;
|
|
|
|
public override void OnEnterPool () {
|
|
nodeLink = default;
|
|
}
|
|
|
|
/// <summary>Works like a constructor, but can be used even for pooled objects. Returns this for easy chaining</summary>
|
|
public RichSpecial Initialize (OffMeshLinks.OffMeshLinkTracer nodeLink) {
|
|
this.nodeLink = nodeLink;
|
|
return this;
|
|
}
|
|
}
|
|
}
|