tschesky/AppleHillsProduction
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[Player] Add AI Pathfinding on a 2D grid to the player character

Browse Source
This commit is contained in:
Michal Pikulski
2025-09-02 11:42:25 +02:00
parent 3d3b72f04d
commit e53b5a0034
663 changed files with 148320 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

547
Assets/External/AstarPathfindingProject/Generators/Utilities/BBTree.cs vendored Normal file
View File

@@ -0,0 +1,547 @@
//#define ASTARDEBUG //"BBTree Debug" If enables, some queries to the tree will show debug lines. Turn off multithreading when using this since DrawLine calls cannot be called from a different thread
using System;
using UnityEngine;
namespace Pathfinding {
using Pathfinding.Util;
/// <summary>
/// Axis Aligned Bounding Box Tree.
/// Holds a bounding box tree of triangles.
/// </summary>
public class BBTree : IAstarPooledObject {
/// <summary>Holds all tree nodes</summary>
BBTreeBox[] tree = null;
TriangleMeshNode[] nodeLookup = null;
int count;
int leafNodes;
const int MaximumLeafSize = 4;
public Rect Size {
get {
if (count == 0) {
return new Rect(0, 0, 0, 0);
} else {
var rect = tree[0].rect;
return Rect.MinMaxRect(rect.xmin*Int3.PrecisionFactor, rect.ymin*Int3.PrecisionFactor, rect.xmax*Int3.PrecisionFactor, rect.ymax*Int3.PrecisionFactor);
}
}
}
/// <summary>
/// Clear the tree.
/// Note that references to old nodes will still be intact so the GC cannot immediately collect them.
/// </summary>
public void Clear () {
count = 0;
leafNodes = 0;
if (tree != null) ArrayPool<BBTreeBox>.Release(ref tree);
if (nodeLookup != null) {
// Prevent memory leaks as the pool does not clear the array
for (int i = 0; i < nodeLookup.Length; i++) nodeLookup[i] = null;
ArrayPool<TriangleMeshNode>.Release(ref nodeLookup);
}
tree = ArrayPool<BBTreeBox>.Claim(0);
nodeLookup = ArrayPool<TriangleMeshNode>.Claim(0);
}
void IAstarPooledObject.OnEnterPool () {
Clear();
}
void EnsureCapacity (int c) {
if (c > tree.Length) {
var newArr = ArrayPool<BBTreeBox>.Claim(c);
tree.CopyTo(newArr, 0);
ArrayPool<BBTreeBox>.Release(ref tree);
tree = newArr;
}
}
void EnsureNodeCapacity (int c) {
if (c > nodeLookup.Length) {
var newArr = ArrayPool<TriangleMeshNode>.Claim(c);
nodeLookup.CopyTo(newArr, 0);
ArrayPool<TriangleMeshNode>.Release(ref nodeLookup);
nodeLookup = newArr;
}
}
int GetBox (IntRect rect) {
if (count >= tree.Length) EnsureCapacity(count+1);
tree[count] = new BBTreeBox(rect);
count++;
return count-1;
}
/// <summary>Rebuilds the tree using the specified nodes</summary>
public void RebuildFrom (TriangleMeshNode[] nodes) {
Clear();
if (nodes.Length == 0) return;
// We will use approximately 2N tree nodes
EnsureCapacity(Mathf.CeilToInt(nodes.Length * 2.1f));
// We will use approximately N node references
EnsureNodeCapacity(Mathf.CeilToInt(nodes.Length * 1.1f));
// This will store the order of the nodes while the tree is being built
// It turns out that it is a lot faster to do this than to actually modify
// the nodes and nodeBounds arrays (presumably since that involves shuffling
// around 20 bytes of memory (sizeof(pointer) + sizeof(IntRect)) per node
// instead of 4 bytes (sizeof(int)).
// It also means we don't have to make a copy of the nodes array since
// we do not modify it
var permutation = ArrayPool<int>.Claim(nodes.Length);
for (int i = 0; i < nodes.Length; i++) {
permutation[i] = i;
}
// Precalculate the bounds of the nodes in XZ space.
// It turns out that calculating the bounds is a bottleneck and precalculating
// the bounds makes it around 3 times faster to build a tree
var nodeBounds = ArrayPool<IntRect>.Claim(nodes.Length);
for (int i = 0; i < nodes.Length; i++) {
Int3 v0, v1, v2;
nodes[i].GetVertices(out v0, out v1, out v2);
var rect = new IntRect(v0.x, v0.z, v0.x, v0.z);
rect = rect.ExpandToContain(v1.x, v1.z);
rect = rect.ExpandToContain(v2.x, v2.z);
nodeBounds[i] = rect;
}
RebuildFromInternal(nodes, permutation, nodeBounds, 0, nodes.Length, false);
ArrayPool<int>.Release(ref permutation);
ArrayPool<IntRect>.Release(ref nodeBounds);
}
static int SplitByX (TriangleMeshNode[] nodes, int[] permutation, int from, int to, int divider) {
int mx = to;
for (int i = from; i < mx; i++) {
if (nodes[permutation[i]].position.x > divider) {
mx--;
// Swap items i and mx
var tmp = permutation[mx];
permutation[mx] = permutation[i];
permutation[i] = tmp;
i--;
}
}
return mx;
}
static int SplitByZ (TriangleMeshNode[] nodes, int[] permutation, int from, int to, int divider) {
int mx = to;
for (int i = from; i < mx; i++) {
if (nodes[permutation[i]].position.z > divider) {
mx--;
// Swap items i and mx
var tmp = permutation[mx];
permutation[mx] = permutation[i];
permutation[i] = tmp;
i--;
}
}
return mx;
}
int RebuildFromInternal (TriangleMeshNode[] nodes, int[] permutation, IntRect[] nodeBounds, int from, int to, bool odd) {
var rect = NodeBounds(permutation, nodeBounds, from, to);
int box = GetBox(rect);
if (to - from <= MaximumLeafSize) {
var nodeOffset = tree[box].nodeOffset = leafNodes*MaximumLeafSize;
EnsureNodeCapacity(nodeOffset + MaximumLeafSize);
leafNodes++;
// Assign all nodes to the array. Note that we also need clear unused slots as the array from the pool may contain any information
for (int i = 0; i < MaximumLeafSize; i++) {
nodeLookup[nodeOffset + i] = i < to - from ? nodes[permutation[from + i]] : null;
}
return box;
}
int splitIndex;
if (odd) {
// X
int divider = (rect.xmin + rect.xmax)/2;
splitIndex = SplitByX(nodes, permutation, from, to, divider);
} else {
// Y/Z
int divider = (rect.ymin + rect.ymax)/2;
splitIndex = SplitByZ(nodes, permutation, from, to, divider);
}
if (splitIndex == from || splitIndex == to) {
// All nodes were on one side of the divider
// Try to split along the other axis
if (!odd) {
// X
int divider = (rect.xmin + rect.xmax)/2;
splitIndex = SplitByX(nodes, permutation, from, to, divider);
} else {
// Y/Z
int divider = (rect.ymin + rect.ymax)/2;
splitIndex = SplitByZ(nodes, permutation, from, to, divider);
}
if (splitIndex == from || splitIndex == to) {
// All nodes were on one side of the divider
// Just pick one half
splitIndex = (from+to)/2;
}
}
tree[box].left = RebuildFromInternal(nodes, permutation, nodeBounds, from, splitIndex, !odd);
tree[box].right = RebuildFromInternal(nodes, permutation, nodeBounds, splitIndex, to, !odd);
return box;
}
/// <summary>Calculates the bounding box in XZ space of all nodes between from (inclusive) and to (exclusive)</summary>
static IntRect NodeBounds (int[] permutation, IntRect[] nodeBounds, int from, int to) {
var rect = nodeBounds[permutation[from]];
for (int j = from + 1; j < to; j++) {
var otherRect = nodeBounds[permutation[j]];
// Equivalent to rect = IntRect.Union(rect, otherRect)
// but manually inlining is approximately
// 25% faster when building an entire tree.
// This code is hot when using navmesh cutting.
rect.xmin = Math.Min(rect.xmin, otherRect.xmin);
rect.ymin = Math.Min(rect.ymin, otherRect.ymin);
rect.xmax = Math.Max(rect.xmax, otherRect.xmax);
rect.ymax = Math.Max(rect.ymax, otherRect.ymax);
}
return rect;
}
[System.Diagnostics.Conditional("ASTARDEBUG")]
static void DrawDebugRect (IntRect rect) {
Debug.DrawLine(new Vector3(rect.xmin, 0, rect.ymin), new Vector3(rect.xmax, 0, rect.ymin), Color.white);
Debug.DrawLine(new Vector3(rect.xmin, 0, rect.ymax), new Vector3(rect.xmax, 0, rect.ymax), Color.white);
Debug.DrawLine(new Vector3(rect.xmin, 0, rect.ymin), new Vector3(rect.xmin, 0, rect.ymax), Color.white);
Debug.DrawLine(new Vector3(rect.xmax, 0, rect.ymin), new Vector3(rect.xmax, 0, rect.ymax), Color.white);
}
[System.Diagnostics.Conditional("ASTARDEBUG")]
static void DrawDebugNode (TriangleMeshNode node, float yoffset, Color color) {
Debug.DrawLine((Vector3)node.GetVertex(1) + Vector3.up*yoffset, (Vector3)node.GetVertex(2) + Vector3.up*yoffset, color);
Debug.DrawLine((Vector3)node.GetVertex(0) + Vector3.up*yoffset, (Vector3)node.GetVertex(1) + Vector3.up*yoffset, color);
Debug.DrawLine((Vector3)node.GetVertex(2) + Vector3.up*yoffset, (Vector3)node.GetVertex(0) + Vector3.up*yoffset, color);
}
/// <summary>
/// Queries the tree for the closest node to p constrained by the NNConstraint.
/// Note that this function will only fill in the constrained node.
/// If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
/// </summary>
public NNInfoInternal QueryClosest (Vector3 p, NNConstraint constraint, out float distance) {
distance = float.PositiveInfinity;
return QueryClosest(p, constraint, ref distance, new NNInfoInternal(null));
}
/// <summary>
/// Queries the tree for the closest node to p constrained by the NNConstraint trying to improve an existing solution.
/// Note that this function will only fill in the constrained node.
/// If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
///
/// This method will completely ignore any Y-axis differences in positions.
/// </summary>
/// <param name="p">Point to search around</param>
/// <param name="constraint">Optionally set to constrain which nodes to return</param>
/// <param name="distance">The best distance for the previous solution. Will be updated with the best distance
/// after this search. Will be positive infinity if no node could be found.
/// Set to positive infinity if there was no previous solution.</param>
/// <param name="previous">This search will start from the previous NNInfo and improve it if possible.
/// Even if the search fails on this call, the solution will never be worse than previous.
/// Note that the distance parameter need to be configured with the distance for the previous result
/// otherwise it may get overwritten even though it was actually closer.</param>
public NNInfoInternal QueryClosestXZ (Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous) {
var sqrDistance = distance*distance;
var origSqrDistance = sqrDistance;
if (count > 0 && SquaredRectPointDistance(tree[0].rect, p) < sqrDistance) {
SearchBoxClosestXZ(0, p, ref sqrDistance, constraint, ref previous);
// Only update the distance if the squared distance changed as otherwise #distance
// might change due to rounding errors even if no better solution was found
if (sqrDistance < origSqrDistance) distance = Mathf.Sqrt(sqrDistance);
}
return previous;
}
void SearchBoxClosestXZ (int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo) {
BBTreeBox box = tree[boxi];
if (box.IsLeaf) {
var nodes = nodeLookup;
for (int i = 0; i < MaximumLeafSize && nodes[box.nodeOffset+i] != null; i++) {
var node = nodes[box.nodeOffset+i];
// Update the NNInfo
DrawDebugNode(node, 0.2f, Color.red);
if (constraint == null || constraint.Suitable(node)) {
Vector3 closest = node.ClosestPointOnNodeXZ(p);
// XZ squared distance
float dist = (closest.x-p.x)*(closest.x-p.x)+(closest.z-p.z)*(closest.z-p.z);
// There's a theoretical case when the closest point is on the edge of a node which may cause the
// closest point's xz coordinates to not line up perfectly with p's xz coordinates even though they should
// (because floating point errors are annoying). So use a tiny margin to cover most of those cases.
const float fuzziness = 0.000001f;
if (nnInfo.constrainedNode == null || dist < closestSqrDist - fuzziness || (dist <= closestSqrDist + fuzziness && Mathf.Abs(closest.y - p.y) < Mathf.Abs(nnInfo.constClampedPosition.y - p.y))) {
nnInfo.constrainedNode = node;
nnInfo.constClampedPosition = closest;
closestSqrDist = dist;
}
}
}
} else {
DrawDebugRect(box.rect);
int first = box.left, second = box.right;
float firstDist, secondDist;
GetOrderedChildren(ref first, ref second, out firstDist, out secondDist, p);
// Search children (closest box first to improve performance)
if (firstDist <= closestSqrDist) {
SearchBoxClosestXZ(first, p, ref closestSqrDist, constraint, ref nnInfo);
}
if (secondDist <= closestSqrDist) {
SearchBoxClosestXZ(second, p, ref closestSqrDist, constraint, ref nnInfo);
}
}
}
/// <summary>
/// Queries the tree for the closest node to p constrained by the NNConstraint trying to improve an existing solution.
/// Note that this function will only fill in the constrained node.
/// If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
/// </summary>
/// <param name="p">Point to search around</param>
/// <param name="constraint">Optionally set to constrain which nodes to return</param>
/// <param name="distance">The best distance for the previous solution. Will be updated with the best distance
/// after this search. Will be positive infinity if no node could be found.
/// Set to positive infinity if there was no previous solution.</param>
/// <param name="previous">This search will start from the previous NNInfo and improve it if possible.
/// Even if the search fails on this call, the solution will never be worse than previous.</param>
public NNInfoInternal QueryClosest (Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous) {
var sqrDistance = distance*distance;
var origSqrDistance = sqrDistance;
if (count > 0 && SquaredRectPointDistance(tree[0].rect, p) < sqrDistance) {
SearchBoxClosest(0, p, ref sqrDistance, constraint, ref previous);
// Only update the distance if the squared distance changed as otherwise #distance
// might change due to rounding errors even if no better solution was found
if (sqrDistance < origSqrDistance) distance = Mathf.Sqrt(sqrDistance);
}
return previous;
}
void SearchBoxClosest (int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo) {
BBTreeBox box = tree[boxi];
if (box.IsLeaf) {
var nodes = nodeLookup;
for (int i = 0; i < MaximumLeafSize && nodes[box.nodeOffset+i] != null; i++) {
var node = nodes[box.nodeOffset+i];
Vector3 closest = node.ClosestPointOnNode(p);
float dist = (closest-p).sqrMagnitude;
if (dist < closestSqrDist) {
DrawDebugNode(node, 0.2f, Color.red);
if (constraint == null || constraint.Suitable(node)) {
// Update the NNInfo
nnInfo.constrainedNode = node;
nnInfo.constClampedPosition = closest;
closestSqrDist = dist;
}
} else {
DrawDebugNode(node, 0.0f, Color.blue);
}
}
} else {
DrawDebugRect(box.rect);
int first = box.left, second = box.right;
float firstDist, secondDist;
GetOrderedChildren(ref first, ref second, out firstDist, out secondDist, p);
// Search children (closest box first to improve performance)
if (firstDist < closestSqrDist) {
SearchBoxClosest(first, p, ref closestSqrDist, constraint, ref nnInfo);
}
if (secondDist < closestSqrDist) {
SearchBoxClosest(second, p, ref closestSqrDist, constraint, ref nnInfo);
}
}
}
/// <summary>Orders the box indices first and second by the approximate distance to the point p</summary>
void GetOrderedChildren (ref int first, ref int second, out float firstDist, out float secondDist, Vector3 p) {
firstDist = SquaredRectPointDistance(tree[first].rect, p);
secondDist = SquaredRectPointDistance(tree[second].rect, p);
if (secondDist < firstDist) {
// Swap
var tmp = first;
first = second;
second = tmp;
var tmp2 = firstDist;
firstDist = secondDist;
secondDist = tmp2;
}
}
/// <summary>
/// Searches for a node which contains the specified point.
/// If there are multiple nodes that contain the point any one of them
/// may be returned.
///
/// See: TriangleMeshNode.ContainsPoint
/// </summary>
public TriangleMeshNode QueryInside (Vector3 p, NNConstraint constraint) {
return count != 0 && tree[0].Contains(p) ? SearchBoxInside(0, p, constraint) : null;
}
TriangleMeshNode SearchBoxInside (int boxi, Vector3 p, NNConstraint constraint) {
BBTreeBox box = tree[boxi];
if (box.IsLeaf) {
var nodes = nodeLookup;
for (int i = 0; i < MaximumLeafSize && nodes[box.nodeOffset+i] != null; i++) {
var node = nodes[box.nodeOffset+i];
if (node.ContainsPoint((Int3)p)) {
DrawDebugNode(node, 0.2f, Color.red);
if (constraint == null || constraint.Suitable(node)) {
return node;
}
} else {
DrawDebugNode(node, 0.0f, Color.blue);
}
}
} else {
DrawDebugRect(box.rect);
//Search children
if (tree[box.left].Contains(p)) {
var result = SearchBoxInside(box.left, p, constraint);
if (result != null) return result;
}
if (tree[box.right].Contains(p)) {
var result = SearchBoxInside(box.right, p, constraint);
if (result != null) return result;
}
}
return null;
}
struct BBTreeBox {
public IntRect rect;
public int nodeOffset;
public int left, right;
public bool IsLeaf {
get {
return nodeOffset >= 0;
}
}
public BBTreeBox (IntRect rect) {
nodeOffset = -1;
this.rect = rect;
left = right = -1;
}
public BBTreeBox (int nodeOffset, IntRect rect) {
this.nodeOffset = nodeOffset;
this.rect = rect;
left = right = -1;
}
public bool Contains (Vector3 point) {
var pi = (Int3)point;
return rect.Contains(pi.x, pi.z);
}
}
public void OnDrawGizmos () {
Gizmos.color = new Color(1, 1, 1, 0.5F);
if (count == 0) return;
OnDrawGizmos(0, 0);
}
void OnDrawGizmos (int boxi, int depth) {
BBTreeBox box = tree[boxi];
var min = (Vector3) new Int3(box.rect.xmin, 0, box.rect.ymin);
var max = (Vector3) new Int3(box.rect.xmax, 0, box.rect.ymax);
Vector3 center = (min+max)*0.5F;
Vector3 size = (max-center)*2;
size = new Vector3(size.x, 1, size.z);
center.y += depth * 2;
Gizmos.color = AstarMath.IntToColor(depth, 1f);
Gizmos.DrawCube(center, size);
if (!box.IsLeaf) {
OnDrawGizmos(box.left, depth + 1);
OnDrawGizmos(box.right, depth + 1);
}
}
static bool NodeIntersectsCircle (TriangleMeshNode node, Vector3 p, float radius) {
if (float.IsPositiveInfinity(radius)) return true;
/// <summary>\bug Is not correct on the Y axis</summary>
return (p - node.ClosestPointOnNode(p)).sqrMagnitude < radius*radius;
}
/// <summary>
/// Returns true if p is within radius from r.
/// Correctly handles cases where radius is positive infinity.
/// </summary>
static bool RectIntersectsCircle (IntRect r, Vector3 p, float radius) {
if (float.IsPositiveInfinity(radius)) return true;
Vector3 po = p;
p.x = Math.Max(p.x, r.xmin*Int3.PrecisionFactor);
p.x = Math.Min(p.x, r.xmax*Int3.PrecisionFactor);
p.z = Math.Max(p.z, r.ymin*Int3.PrecisionFactor);
p.z = Math.Min(p.z, r.ymax*Int3.PrecisionFactor);
// XZ squared magnitude comparison
return (p.x-po.x)*(p.x-po.x) + (p.z-po.z)*(p.z-po.z) < radius*radius;
}
/// <summary>Returns distance from p to the rectangle r</summary>
static float SquaredRectPointDistance (IntRect r, Vector3 p) {
Vector3 po = p;
p.x = Math.Max(p.x, r.xmin*Int3.PrecisionFactor);
p.x = Math.Min(p.x, r.xmax*Int3.PrecisionFactor);
p.z = Math.Max(p.z, r.ymin*Int3.PrecisionFactor);
p.z = Math.Min(p.z, r.ymax*Int3.PrecisionFactor);
// XZ squared magnitude comparison
return (p.x-po.x)*(p.x-po.x) + (p.z-po.z)*(p.z-po.z);
}
}
}