diff options
Diffstat (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities')
12 files changed, 1950 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs new file mode 100644 index 0000000..feb8fa7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs @@ -0,0 +1,442 @@ +#pragma warning disable 414 +using System.Collections.Generic; +using UnityEngine; +using Unity.Mathematics; +using Unity.Collections; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Util { + using Pathfinding.Drawing; + + public enum HeuristicOptimizationMode { + None, + Random, + RandomSpreadOut, + Custom + } + + /// <summary> + /// Implements heuristic optimizations. + /// + /// See: heuristic-opt + /// See: Game AI Pro - Pathfinding Architecture Optimizations by Steve Rabin and Nathan R. Sturtevant + /// </summary> + [System.Serializable] + public class EuclideanEmbedding { + /// <summary> + /// If heuristic optimization should be used and how to place the pivot points. + /// See: heuristic-opt + /// See: Game AI Pro - Pathfinding Architecture Optimizations by Steve Rabin and Nathan R. Sturtevant + /// </summary> + public HeuristicOptimizationMode mode; + + public int seed; + + /// <summary>All children of this transform will be used as pivot points</summary> + public Transform pivotPointRoot; + + public int spreadOutCount = 1; + + [System.NonSerialized] + public bool dirty; + + /// <summary> + /// Costs laid out as n*[int],n*[int],n*[int] where n is the number of pivot points. + /// Each node has n integers which is the cost from that node to the pivot node. + /// They are at around the same place in the array for simplicity and for cache locality. + /// + /// cost(nodeIndex, pivotIndex) = costs[nodeIndex*pivotCount+pivotIndex] + /// </summary> + public NativeArray<uint> costs { get; private set; } + public int pivotCount { get; private set; } + + GraphNode[] pivots; + + /* + * Seed for random number generator. + * Must not be zero + */ + const uint ra = 12820163; + + /* + * Seed for random number generator. + * Must not be zero + */ + const uint rc = 1140671485; + + /* + * Parameter for random number generator. + */ + uint rval; + + /// <summary> + /// Simple linear congruential generator. + /// See: http://en.wikipedia.org/wiki/Linear_congruential_generator + /// </summary> + uint GetRandom () { + rval = (ra*rval + rc); + return rval; + } + public void OnDisable () { + if (costs.IsCreated) costs.Dispose(); + costs = default; + pivotCount = 0; + } + + public static uint GetHeuristic (UnsafeSpan<uint> costs, uint pivotCount, uint nodeIndex1, uint nodeIndex2) { + uint mx = 0; + // TODO: Force pivotCount to be a multiple of 4 and use SIMD for performance + if (nodeIndex1 < costs.Length && nodeIndex2 < costs.Length) { + for (uint i = 0; i < pivotCount; i++) { + var c1 = costs[nodeIndex1*pivotCount+i]; + var c2 = costs[nodeIndex2*pivotCount+i]; + // If either of the nodes have an unknown cost to the pivot point, + // then we cannot use this pivot to calculate a heuristic + if (c1 == uint.MaxValue || c2 == uint.MaxValue) continue; + uint d = (uint)math.abs((int)c1 - (int)c2); + if (d > mx) mx = d; + } + } + return mx; + } + + void GetClosestWalkableNodesToChildrenRecursively (Transform tr, List<GraphNode> nodes) { + foreach (Transform ch in tr) { + var info = AstarPath.active.GetNearest(ch.position, NNConstraint.Walkable); + if (info.node != null && info.node.Walkable) { + nodes.Add(info.node); + } + + GetClosestWalkableNodesToChildrenRecursively(ch, nodes); + } + } + + /// <summary> + /// Pick N random walkable nodes from all nodes in all graphs and add them to the buffer. + /// + /// Here we select N random nodes from a stream of nodes. + /// Probability of choosing the first N nodes is 1 + /// Probability of choosing node i is min(N/i,1) + /// A selected node will replace a random node of the previously + /// selected ones. + /// + /// See: https://en.wikipedia.org/wiki/Reservoir_sampling + /// </summary> + void PickNRandomNodes (int count, List<GraphNode> buffer) { + int n = 0; + + var graphs = AstarPath.active.graphs; + + // Loop through all graphs + for (int j = 0; j < graphs.Length; j++) { + // Loop through all nodes in the graph + graphs[j].GetNodes(node => { + if (!node.Destroyed && node.Walkable) { + n++; + if ((GetRandom() % n) < count) { + if (buffer.Count < count) { + buffer.Add(node); + } else { + buffer[(int)(n%buffer.Count)] = node; + } + } + } + }); + } + } + + GraphNode PickAnyWalkableNode () { + var graphs = AstarPath.active.graphs; + GraphNode first = null; + + // Find any node in the graphs + for (int j = 0; j < graphs.Length; j++) { + graphs[j].GetNodes(node => { + if (node != null && node.Walkable && first == null) { + first = node; + } + }); + } + + return first; + } + + public void RecalculatePivots () { + if (mode == HeuristicOptimizationMode.None) { + pivotCount = 0; + pivots = null; + return; + } + + // Reset the random number generator + rval = (uint)seed; + + // Get a List<GraphNode> from a pool + var pivotList = Pathfinding.Util.ListPool<GraphNode>.Claim(); + + switch (mode) { + case HeuristicOptimizationMode.Custom: + if (pivotPointRoot == null) throw new System.Exception("heuristicOptimizationMode is HeuristicOptimizationMode.Custom, " + + "but no 'customHeuristicOptimizationPivotsRoot' is set"); + + GetClosestWalkableNodesToChildrenRecursively(pivotPointRoot, pivotList); + break; + case HeuristicOptimizationMode.Random: + PickNRandomNodes(spreadOutCount, pivotList); + break; + case HeuristicOptimizationMode.RandomSpreadOut: + if (pivotPointRoot != null) { + GetClosestWalkableNodesToChildrenRecursively(pivotPointRoot, pivotList); + } + + // If no pivot points were found, fall back to picking arbitrary nodes + if (pivotList.Count == 0) { + GraphNode first = PickAnyWalkableNode(); + + if (first != null) { + pivotList.Add(first); + } else { + Debug.LogError("Could not find any walkable node in any of the graphs."); + Pathfinding.Util.ListPool<GraphNode>.Release(ref pivotList); + return; + } + } + + // Fill remaining slots with null + int toFill = spreadOutCount - pivotList.Count; + for (int i = 0; i < toFill; i++) pivotList.Add(null); + break; + default: + throw new System.Exception("Invalid HeuristicOptimizationMode: " + mode); + } + + pivots = pivotList.ToArray(); + + Pathfinding.Util.ListPool<GraphNode>.Release(ref pivotList); + } + + class EuclideanEmbeddingSearchPath : Path { + public UnsafeSpan<uint> costs; + public uint costIndexStride; + public uint pivotIndex; + public GraphNode startNode; + public uint furthestNodeScore; + public GraphNode furthestNode; + + public static EuclideanEmbeddingSearchPath Construct (UnsafeSpan<uint> costs, uint costIndexStride, uint pivotIndex, GraphNode startNode) { + var p = PathPool.GetPath<EuclideanEmbeddingSearchPath>(); + p.costs = costs; + p.costIndexStride = costIndexStride; + p.pivotIndex = pivotIndex; + p.startNode = startNode; + p.furthestNodeScore = 0; + p.furthestNode = null; + return p; + } + + protected override void OnFoundEndNode (uint pathNode, uint hScore, uint gScore) { + throw new System.InvalidOperationException(); + } + + protected override void OnHeapExhausted () { + CompleteState = PathCompleteState.Complete; + } + + public override void OnVisitNode (uint pathNode, uint hScore, uint gScore) { + if (!pathHandler.IsTemporaryNode(pathNode)) { + // Get the node and then the node index from that. + // This is because a triangle mesh node will have 3 path nodes, + // but we want to collapse those to the same index as the original node. + var node = pathHandler.GetNode(pathNode); + uint baseIndex = node.NodeIndex*costIndexStride; + // EnsureCapacity(idx); + + costs[baseIndex + pivotIndex] = math.min(costs[baseIndex + pivotIndex], gScore); + + // Find the minimum distance from the node to all existing pivot points + uint mx = uint.MaxValue; + for (int p = 0; p <= pivotIndex; p++) mx = math.min(mx, costs[baseIndex + (uint)p]); + + // Pick the node which has the largest minimum distance to the existing pivot points + // (i.e pick the one furthest away from the existing ones) + if (mx > furthestNodeScore || furthestNode == null) { + furthestNodeScore = mx; + furthestNode = node; + } + } + } + + protected override void Prepare () { + pathHandler.AddTemporaryNode(new TemporaryNode { + associatedNode = startNode.NodeIndex, + position = startNode.position, + type = TemporaryNodeType.Start, + }); + heuristicObjective = new HeuristicObjective(0, Heuristic.None, 0.0f); + MarkNodesAdjacentToTemporaryEndNodes(); + AddStartNodesToHeap(); + } + } + + public void RecalculateCosts () { + if (pivots == null) RecalculatePivots(); + if (mode == HeuristicOptimizationMode.None) return; + + // Use a nested call to avoid allocating a delegate object + // even when we just do an early return. + RecalculateCostsInner(); + } + + void RecalculateCostsInner () { + pivotCount = 0; + + for (int i = 0; i < pivots.Length; i++) { + if (pivots[i] != null && (pivots[i].Destroyed || !pivots[i].Walkable)) { + throw new System.Exception("Invalid pivot nodes (destroyed or unwalkable)"); + } + } + + if (mode != HeuristicOptimizationMode.RandomSpreadOut) + for (int i = 0; i < pivots.Length; i++) + if (pivots[i] == null) + throw new System.Exception("Invalid pivot nodes (null)"); + + pivotCount = pivots.Length; + + System.Action<int> startCostCalculation = null; + + int numComplete = 0; + + var nodeCount = AstarPath.active.nodeStorage.nextNodeIndex; + if (costs.IsCreated) costs.Dispose(); + // TODO: Quantize costs a bit to reduce memory usage? + costs = new NativeArray<uint>((int)nodeCount * pivotCount, Allocator.Persistent); + costs.AsUnsafeSpan().Fill(uint.MaxValue); + + startCostCalculation = (int pivotIndex) => { + GraphNode pivot = pivots[pivotIndex]; + + var path = EuclideanEmbeddingSearchPath.Construct( + costs.AsUnsafeSpan(), + (uint)pivotCount, + (uint)pivotIndex, + pivot + ); + + path.immediateCallback = (Path _) => { + if (mode == HeuristicOptimizationMode.RandomSpreadOut && pivotIndex < pivots.Length-1) { + // If the next pivot is null + // then find the node which is furthest away from the earlier + // pivot points + if (pivots[pivotIndex+1] == null) { + pivots[pivotIndex+1] = path.furthestNode; + + if (path.furthestNode == null) { + Debug.LogError("Failed generating random pivot points for heuristic optimizations"); + return; + } + } + + // Start next path + startCostCalculation(pivotIndex+1); + } + + numComplete++; + if (numComplete == pivotCount) { + // Last completed path + ApplyGridGraphEndpointSpecialCase(); + } + }; + + AstarPath.StartPath(path, true, true); + }; + + if (mode != HeuristicOptimizationMode.RandomSpreadOut) { + // All calculated in parallel + for (int i = 0; i < pivots.Length; i++) { + startCostCalculation(i); + } + } else { + // Recursive and serial + startCostCalculation(0); + } + + dirty = false; + } + + /// <summary> + /// Special case necessary for paths to unwalkable nodes right next to walkable nodes to be able to use good heuristics. + /// + /// This will find all unwalkable nodes in all grid graphs with walkable nodes as neighbours + /// and set the cost to reach them from each of the pivots as the minimum of the cost to + /// reach the neighbours of each node. + /// + /// See: ABPath.EndPointGridGraphSpecialCase + /// </summary> + void ApplyGridGraphEndpointSpecialCase () { + var costs = this.costs.AsUnsafeSpan(); +#if !ASTAR_NO_GRID_GRAPH + var graphs = AstarPath.active.graphs; + for (int i = 0; i < graphs.Length; i++) { + if (graphs[i] is GridGraph gg) { + // Found a grid graph + var nodes = gg.nodes; + + // Number of neighbours as an int + int mxnum = gg.neighbours == NumNeighbours.Four ? 4 : (gg.neighbours == NumNeighbours.Eight ? 8 : 6); + + for (int z = 0; z < gg.depth; z++) { + for (int x = 0; x < gg.width; x++) { + var node = nodes[z*gg.width + x]; + if (!node.Walkable) { + var pivotIndex = node.NodeIndex*(uint)pivotCount; + // Set all costs to reach this node to maximum + for (int piv = 0; piv < pivotCount; piv++) { + costs[pivotIndex + (uint)piv] = uint.MaxValue; + } + + // Loop through all potential neighbours of the node + // and set the cost to reach it as the minimum + // of the costs to reach any of the adjacent nodes + for (int d = 0; d < mxnum; d++) { + int nx, nz; + if (gg.neighbours == NumNeighbours.Six) { + // Hexagon graph + nx = x + GridGraph.neighbourXOffsets[GridGraph.hexagonNeighbourIndices[d]]; + nz = z + GridGraph.neighbourZOffsets[GridGraph.hexagonNeighbourIndices[d]]; + } else { + nx = x + GridGraph.neighbourXOffsets[d]; + nz = z + GridGraph.neighbourZOffsets[d]; + } + + // Check if the position is still inside the grid + if (nx >= 0 && nz >= 0 && nx < gg.width && nz < gg.depth) { + var adjacentNode = gg.nodes[nz*gg.width + nx]; + if (adjacentNode.Walkable) { + for (uint piv = 0; piv < pivotCount; piv++) { + uint cost = costs[adjacentNode.NodeIndex*(uint)pivotCount + piv] + gg.neighbourCosts[d]; + costs[pivotIndex + piv] = System.Math.Min(costs[pivotIndex + piv], cost); + //Debug.DrawLine((Vector3)node.position, (Vector3)adjacentNode.position, Color.blue, 1); + } + } + } + } + } + } + } + } + } +#endif + } + + public void OnDrawGizmos () { + if (pivots != null) { + for (int i = 0; i < pivots.Length; i++) { + if (pivots[i] != null && !pivots[i].Destroyed) { + Draw.SolidBox((Vector3)pivots[i].position, Vector3.one, new Color(159/255.0f, 94/255.0f, 194/255.0f, 0.8f)); + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs.meta new file mode 100644 index 0000000..dd2f06c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 3ac3213e3eeb14eef91939f5281682e6 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs new file mode 100644 index 0000000..3e25b20 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs @@ -0,0 +1,575 @@ +using Unity.Mathematics; +using UnityEngine; + +namespace Pathfinding.Util { + /// <summary> + /// Transforms to and from world space to a 2D movement plane. + /// The transformation is guaranteed to be purely a rotation + /// so no scale or offset is used. This interface is primarily + /// used to make it easier to write movement scripts which can + /// handle movement both in the XZ plane and in the XY plane. + /// + /// See: <see cref="Pathfinding.Util.GraphTransform"/> + /// </summary> + public interface IMovementPlane { + Vector2 ToPlane(Vector3 p); + Vector2 ToPlane(Vector3 p, out float elevation); + Vector3 ToWorld(Vector2 p, float elevation = 0); + SimpleMovementPlane ToSimpleMovementPlane(); + } + + /// <summary> + /// A matrix wrapper which can be used to project points from world space to a movement plane. + /// + /// In contrast to <see cref="NativeMovementPlane"/>, this is represented by a matrix instead of a quaternion. + /// This means it is less space efficient (36 bytes instead of 16 bytes) but it is more performant when + /// you need to do a lot of ToPlane conversions. + /// </summary> + public readonly struct ToPlaneMatrix { + public readonly float3x3 matrix; + + public ToPlaneMatrix (NativeMovementPlane plane) => this.matrix = new float3x3(math.conjugate(plane.rotation)); + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// See: <see cref="NativeMovementPlane.ToPlane(float3)"/> + /// </summary> + public float2 ToPlane(float3 p) => math.mul(matrix, p).xz; + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// The elevation coordinate will be returned as the y coordinate of the returned vector. + /// + /// See: <see cref="NativeMovementPlane.ToPlane(float3)"/> + /// </summary> + public float3 ToXZPlane(float3 p) => math.mul(matrix, p); + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// See: <see cref="NativeMovementPlane.ToPlane(float3)"/> + /// </summary> + public float2 ToPlane (float3 p, out float elevation) { + var v = math.mul(matrix, p); + elevation = v.y; + return v.xz; + } + } + + /// <summary> + /// A matrix wrapper which can be used to project points from a movement plane to world space. + /// + /// In contrast to <see cref="NativeMovementPlane"/>, this is represented by a matrix instead of a quaternion. + /// This means it is less space efficient (36 bytes instead of 16 bytes) but it is more performant when + /// you need to do a lot of ToWorld conversions. + /// </summary> + public readonly struct ToWorldMatrix { + public readonly float3x3 matrix; + + public ToWorldMatrix (NativeMovementPlane plane) => this.matrix = new float3x3(plane.rotation); + + public ToWorldMatrix (float3x3 matrix) => this.matrix = matrix; + + public float3 ToWorld(float2 p, float elevation = 0) => math.mul(matrix, new float3(p.x, elevation, p.y)); + + /// <summary> + /// Transforms a bounding box from local space to world space. + /// + /// The Y coordinate of the bounding box is the elevation coordinate. + /// + /// See: https://zeux.io/2010/10/17/aabb-from-obb-with-component-wise-abs/ + /// </summary> + public Bounds ToWorld (Bounds bounds) { + Bounds result = default; + result.center = math.mul(matrix, (float3)bounds.center); + result.extents = math.mul(new float3x3( + math.abs(matrix.c0), + math.abs(matrix.c1), + math.abs(matrix.c2) + ), (float3)bounds.extents); + return result; + } + } + + /// <summary>A variant of <see cref="SimpleMovementPlane"/> that can be passed to burst functions</summary> + public readonly struct NativeMovementPlane { + /// <summary> + /// The rotation of the plane. + /// The plane is defined by the XZ-plane rotated by this quaternion. + /// + /// Should always be normalized. + /// </summary> + public readonly quaternion rotation; + + /// <summary>Normal of the plane</summary> + // TODO: Check constructor for float3x3(quaternion), seems smarter, at least in burst + public float3 up => 2 * new float3(rotation.value.x * rotation.value.y - rotation.value.w * rotation.value.z, 0.5f - rotation.value.x * rotation.value.x - rotation.value.z * rotation.value.z, rotation.value.w * rotation.value.x + rotation.value.y * rotation.value.z); // math.mul(rotation, Vector3.up); + + public NativeMovementPlane(quaternion rotation) { + // We need to normalize to make sure that math.inverse(rotation) == math.conjugate(rotation). + // We want to use conjugate because it's faster. + this.rotation = math.normalizesafe(rotation); + } + + public NativeMovementPlane(SimpleMovementPlane plane) : this(plane.rotation) {} + + public ToPlaneMatrix AsWorldToPlaneMatrix() => new ToPlaneMatrix(this); + public ToWorldMatrix AsPlaneToWorldMatrix() => new ToWorldMatrix(this); + + public float ProjectedLength(float3 v) => math.length(ToPlane(v)); + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// For a graph rotated with the rotation (-90, 0, 0) this will transform + /// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0) + /// this will tranform a coordinate (x,y,z) to (x,z). More generally for + /// a graph with a quaternion rotation R this will transform a vector V + /// to inverse(R) * V (i.e rotate the vector V using the inverse of rotation R). + /// </summary> + public float2 ToPlane (float3 p) { + return math.mul(math.conjugate(rotation), p).xz; + } + + /// <summary>Transforms from world space to the 'ground' plane of the graph</summary> + public float2 ToPlane (float3 p, out float elevation) { + p = math.mul(math.conjugate(rotation), p); + elevation = p.y; + return p.xz; + } + + /// <summary> + /// Transforms from the 'ground' plane of the graph to world space. + /// The transformation is purely a rotation so no scale or offset is used. + /// </summary> + public float3 ToWorld (float2 p, float elevation = 0f) { + return math.mul(rotation, new float3(p.x, elevation, p.y)); + } + + /// <summary> + /// Projects a rotation onto the plane. + /// + /// The returned angle is such that + /// + /// <code> + /// var angle = ...; + /// var q = math.mul(plane.rotation, quaternion.RotateY(angle)); + /// AstarMath.DeltaAngle(plane.ToPlane(q), -angle) == 0; // or at least approximately equal + /// </code> + /// + /// See: <see cref="ToWorldRotation"/> + /// See: <see cref="ToWorldRotationDelta"/> + /// </summary> + /// <param name="rotation">the rotation to project</param> + public float ToPlane (quaternion rotation) { + var inPlaneRotation = math.mul(math.conjugate(this.rotation), rotation); + // Ensure the rotation axis is always along +Y + if (inPlaneRotation.value.y < 0) inPlaneRotation.value = -inPlaneRotation.value; + var twist = math.normalizesafe(new quaternion(0, inPlaneRotation.value.y, 0, inPlaneRotation.value.w)); + return -VectorMath.QuaternionAngle(twist); + } + + public quaternion ToWorldRotation (float angle) { + return math.mul(rotation, quaternion.RotateY(-angle)); + } + + public quaternion ToWorldRotationDelta (float deltaAngle) { + return quaternion.AxisAngle(ToWorld(float2.zero, 1), -deltaAngle); + } + + /// <summary> + /// Transforms a bounding box from local space to world space. + /// + /// The Y coordinate of the bounding box is the elevation coordinate. + /// </summary> + public Bounds ToWorld(Bounds bounds) => AsPlaneToWorldMatrix().ToWorld(bounds); + } + + /// <summary> + /// Represents the orientation of a plane. + /// + /// When a character walks around in the world, it may not necessarily walk on the XZ-plane. + /// It may be the case that the character is on a spherical world, or maybe it walks on a wall or upside down on the ceiling. + /// + /// A movement plane is used to handle this. It contains functions for converting a 3D point into a 2D point on that plane, and functions for converting back to 3D. + /// + /// See: NativeMovementPlane + /// </summary> +#if MODULE_COLLECTIONS_2_0_0_OR_NEWER && UNITY_2022_2_OR_NEWER + [Unity.Collections.GenerateTestsForBurstCompatibility] +#endif + public readonly struct SimpleMovementPlane : IMovementPlane { + public readonly Quaternion rotation; + public readonly Quaternion inverseRotation; + readonly byte plane; + public bool isXY => plane == 1; + public bool isXZ => plane == 2; + + /// <summary>A plane that spans the X and Y axes</summary> + public static readonly SimpleMovementPlane XYPlane = new SimpleMovementPlane(Quaternion.Euler(-90, 0, 0)); + + /// <summary>A plane that spans the X and Z axes</summary> + public static readonly SimpleMovementPlane XZPlane = new SimpleMovementPlane(Quaternion.identity); + + public SimpleMovementPlane (Quaternion rotation) { + this.rotation = rotation; + // TODO: Normalize #rotation and compute inverse every time instead (less memory) + inverseRotation = Quaternion.Inverse(rotation); + // Some short circuiting code for the movement plane calculations + if (rotation == XYPlane.rotation) plane = 1; + else if (rotation == Quaternion.identity) plane = 2; + else plane = 0; + } + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// For a graph rotated with the rotation (-90, 0, 0) this will transform + /// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0) + /// this will tranform a coordinate (x,y,z) to (x,z). More generally for + /// a graph with a quaternion rotation R this will transform a vector V + /// to inverse(R) * V (i.e rotate the vector V using the inverse of rotation R). + /// </summary> + public Vector2 ToPlane (Vector3 point) { + // These special cases cover most graph orientations used in practice. + // Having them here improves performance in those cases by a factor of + // 2.5 without impacting the generic case in any significant way. + if (isXY) return new Vector2(point.x, point.y); + if (!isXZ) point = inverseRotation * point; + return new Vector2(point.x, point.z); + } + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// For a graph rotated with the rotation (-90, 0, 0) this will transform + /// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0) + /// this will tranform a coordinate (x,y,z) to (x,z). More generally for + /// a graph with a quaternion rotation R this will transform a vector V + /// to inverse(R) * V (i.e rotate the vector V using the inverse of rotation R). + /// </summary> + public float2 ToPlane (float3 point) { + return ((float3)(inverseRotation * (Vector3)point)).xz; + } + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// </summary> + public Vector2 ToPlane (Vector3 point, out float elevation) { + if (!isXZ) point = inverseRotation * point; + elevation = point.y; + return new Vector2(point.x, point.z); + } + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// </summary> + public float2 ToPlane (float3 point, out float elevation) { + point = math.mul(inverseRotation, point); + elevation = point.y; + return point.xz; + } + + /// <summary> + /// Transforms from the 'ground' plane of the graph to world space. + /// The transformation is purely a rotation so no scale or offset is used. + /// </summary> + public Vector3 ToWorld (Vector2 point, float elevation = 0) { + return rotation * new Vector3(point.x, elevation, point.y); + } + + /// <summary> + /// Transforms from the 'ground' plane of the graph to world space. + /// The transformation is purely a rotation so no scale or offset is used. + /// </summary> + public float3 ToWorld (float2 point, float elevation = 0) { + return rotation * new Vector3(point.x, elevation, point.y); + } + + public SimpleMovementPlane ToSimpleMovementPlane () { + return this; + } + + public static bool operator== (SimpleMovementPlane lhs, SimpleMovementPlane rhs) { + return lhs.rotation == rhs.rotation; + } + + public static bool operator!= (SimpleMovementPlane lhs, SimpleMovementPlane rhs) { + return lhs.rotation != rhs.rotation; + } + + public override bool Equals (System.Object other) { + if (!(other is SimpleMovementPlane)) return false; + return rotation == ((SimpleMovementPlane)other).rotation; + } + + public override int GetHashCode () { + return rotation.GetHashCode(); + } + } + + /// <summary>Generic 3D coordinate transformation</summary> + public interface ITransform { + Vector3 Transform(Vector3 position); + Vector3 InverseTransform(Vector3 position); + } + + /// <summary>Like <see cref="Pathfinding.Util.GraphTransform"/>, but mutable</summary> + public class MutableGraphTransform : GraphTransform { + public MutableGraphTransform (Matrix4x4 matrix) : base(matrix) {} + + /// <summary>Replace this transform with the given matrix transformation</summary> + public void SetMatrix (Matrix4x4 matrix) { + Set(matrix); + } + } + + /// <summary> + /// Defines a transformation from graph space to world space. + /// This is essentially just a simple wrapper around a matrix, but it has several utilities that are useful. + /// </summary> + public class GraphTransform : IMovementPlane, ITransform { + /// <summary>True if this transform is the identity transform (i.e it does not do anything)</summary> + public bool identity { get { return isIdentity; } } + + /// <summary>True if this transform is a pure translation without any scaling or rotation</summary> + public bool onlyTranslational { get { return isOnlyTranslational; } } + + bool isXY; + bool isXZ; + bool isOnlyTranslational; + bool isIdentity; + + public Matrix4x4 matrix { get; private set; } + public Matrix4x4 inverseMatrix { get; private set; } + Vector3 up; + Vector3 translation; + Int3 i3translation; + public Quaternion rotation { get; private set; } + Quaternion inverseRotation; + + public static readonly GraphTransform identityTransform = new GraphTransform(Matrix4x4.identity); + + /// <summary>Transforms from the XZ plane to the XY plane</summary> + public static readonly GraphTransform xyPlane = new GraphTransform(Matrix4x4.TRS(Vector3.zero, Quaternion.Euler(-90, 0, 0), Vector3.one)); + + /// <summary>Transforms from the XZ plane to the XZ plane (i.e. an identity transformation)</summary> + public static readonly GraphTransform xzPlane = new GraphTransform(Matrix4x4.identity); + + public GraphTransform (Matrix4x4 matrix) { + Set(matrix); + } + + protected void Set (Matrix4x4 matrix) { + this.matrix = matrix; + inverseMatrix = matrix.inverse; + isIdentity = matrix.isIdentity; + isOnlyTranslational = MatrixIsTranslational(matrix); + up = matrix.MultiplyVector(Vector3.up).normalized; + translation = matrix.MultiplyPoint3x4(Vector3.zero); + i3translation = (Int3)translation; + + // Extract the rotation from the matrix. This is only correct if the matrix has no skew, but we only + // want to use it for the movement plane so as long as the Up axis is parpendicular to the Forward + // axis everything should be ok. In fact the only case in the project when all three axes are not + // perpendicular is when hexagon or isometric grid graphs are used, but in those cases only the + // X and Z axes are not perpendicular. + rotation = Quaternion.LookRotation(TransformVector(Vector3.forward), TransformVector(Vector3.up)); + inverseRotation = Quaternion.Inverse(rotation); + // Some short circuiting code for the movement plane calculations + isXY = rotation == Quaternion.Euler(-90, 0, 0); + isXZ = rotation == Quaternion.Euler(0, 0, 0); + } + + public Vector3 WorldUpAtGraphPosition (Vector3 point) { + return up; + } + + static bool MatrixIsTranslational (Matrix4x4 matrix) { + return matrix.GetColumn(0) == new Vector4(1, 0, 0, 0) && matrix.GetColumn(1) == new Vector4(0, 1, 0, 0) && matrix.GetColumn(2) == new Vector4(0, 0, 1, 0) && matrix.m33 == 1; + } + + public Vector3 Transform (Vector3 point) { + if (onlyTranslational) return point + translation; + return matrix.MultiplyPoint3x4(point); + } + + public Vector3 TransformVector (Vector3 dir) { + if (onlyTranslational) return dir; + return matrix.MultiplyVector(dir); + } + + public void Transform (Int3[] arr) { + if (onlyTranslational) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] += i3translation; + } else { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)matrix.MultiplyPoint3x4((Vector3)arr[i]); + } + } + + public void Transform (UnsafeSpan<Int3> arr) { + if (onlyTranslational) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] += i3translation; + } else { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)matrix.MultiplyPoint3x4((Vector3)arr[i]); + } + } + + public void Transform (Vector3[] arr) { + if (onlyTranslational) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] += translation; + } else { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = matrix.MultiplyPoint3x4(arr[i]); + } + } + + public Vector3 InverseTransform (Vector3 point) { + if (onlyTranslational) return point - translation; + return inverseMatrix.MultiplyPoint3x4(point); + } + + public Vector3 InverseTransformVector (Vector3 dir) { + if (onlyTranslational) return dir; + return inverseMatrix.MultiplyVector(dir); + } + + public Int3 InverseTransform (Int3 point) { + if (onlyTranslational) return point - i3translation; + return (Int3)inverseMatrix.MultiplyPoint3x4((Vector3)point); + } + + public void InverseTransform (Int3[] arr) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)inverseMatrix.MultiplyPoint3x4((Vector3)arr[i]); + } + + public void InverseTransform (UnsafeSpan<Int3> arr) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)inverseMatrix.MultiplyPoint3x4((Vector3)arr[i]); + } + + public static GraphTransform operator * (GraphTransform lhs, Matrix4x4 rhs) { + return new GraphTransform(lhs.matrix * rhs); + } + + public static GraphTransform operator * (Matrix4x4 lhs, GraphTransform rhs) { + return new GraphTransform(lhs * rhs.matrix); + } + + public Bounds Transform (Bounds bounds) { + if (onlyTranslational) return new Bounds(bounds.center + translation, bounds.size); + + var corners = ArrayPool<Vector3>.Claim(8); + var extents = bounds.extents; + corners[0] = Transform(bounds.center + new Vector3(extents.x, extents.y, extents.z)); + corners[1] = Transform(bounds.center + new Vector3(extents.x, extents.y, -extents.z)); + corners[2] = Transform(bounds.center + new Vector3(extents.x, -extents.y, extents.z)); + corners[3] = Transform(bounds.center + new Vector3(extents.x, -extents.y, -extents.z)); + corners[4] = Transform(bounds.center + new Vector3(-extents.x, extents.y, extents.z)); + corners[5] = Transform(bounds.center + new Vector3(-extents.x, extents.y, -extents.z)); + corners[6] = Transform(bounds.center + new Vector3(-extents.x, -extents.y, extents.z)); + corners[7] = Transform(bounds.center + new Vector3(-extents.x, -extents.y, -extents.z)); + + var min = corners[0]; + var max = corners[0]; + for (int i = 1; i < 8; i++) { + min = Vector3.Min(min, corners[i]); + max = Vector3.Max(max, corners[i]); + } + ArrayPool<Vector3>.Release(ref corners); + return new Bounds((min+max)*0.5f, max - min); + } + + public Bounds InverseTransform (Bounds bounds) { + if (onlyTranslational) return new Bounds(bounds.center - translation, bounds.size); + + var corners = ArrayPool<Vector3>.Claim(8); + var extents = bounds.extents; + corners[0] = InverseTransform(bounds.center + new Vector3(extents.x, extents.y, extents.z)); + corners[1] = InverseTransform(bounds.center + new Vector3(extents.x, extents.y, -extents.z)); + corners[2] = InverseTransform(bounds.center + new Vector3(extents.x, -extents.y, extents.z)); + corners[3] = InverseTransform(bounds.center + new Vector3(extents.x, -extents.y, -extents.z)); + corners[4] = InverseTransform(bounds.center + new Vector3(-extents.x, extents.y, extents.z)); + corners[5] = InverseTransform(bounds.center + new Vector3(-extents.x, extents.y, -extents.z)); + corners[6] = InverseTransform(bounds.center + new Vector3(-extents.x, -extents.y, extents.z)); + corners[7] = InverseTransform(bounds.center + new Vector3(-extents.x, -extents.y, -extents.z)); + + var min = corners[0]; + var max = corners[0]; + for (int i = 1; i < 8; i++) { + min = Vector3.Min(min, corners[i]); + max = Vector3.Max(max, corners[i]); + } + ArrayPool<Vector3>.Release(ref corners); + return new Bounds((min+max)*0.5f, max - min); + } + + #region IMovementPlane implementation + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// For a graph rotated with the rotation (-90, 0, 0) this will transform + /// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0) + /// this will tranform a coordinate (x,y,z) to (x,z). More generally for + /// a graph with a quaternion rotation R this will transform a vector V + /// to R * V (i.e rotate the vector V using the rotation R). + /// </summary> + Vector2 IMovementPlane.ToPlane (Vector3 point) { + // These special cases cover most graph orientations used in practice. + // Having them here improves performance in those cases by a factor of + // 2.5 without impacting the generic case in any significant way. + if (isXY) return new Vector2(point.x, point.y); + if (!isXZ) point = inverseRotation * point; + return new Vector2(point.x, point.z); + } + + /// <summary> + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// </summary> + Vector2 IMovementPlane.ToPlane (Vector3 point, out float elevation) { + if (!isXZ) point = inverseRotation * point; + elevation = point.y; + return new Vector2(point.x, point.z); + } + + /// <summary> + /// Transforms from the 'ground' plane of the graph to world space. + /// The transformation is purely a rotation so no scale or offset is used. + /// </summary> + Vector3 IMovementPlane.ToWorld (Vector2 point, float elevation) { + return rotation * new Vector3(point.x, elevation, point.y); + } + + public SimpleMovementPlane ToSimpleMovementPlane () { + return new SimpleMovementPlane(rotation); + } + + #endregion + + /// <summary>Copies the data in this transform to another mutable graph transform</summary> + public void CopyTo (MutableGraphTransform graphTransform) { + graphTransform.isXY = isXY; + graphTransform.isXZ = isXZ; + graphTransform.isOnlyTranslational = isOnlyTranslational; + graphTransform.isIdentity = isIdentity; + graphTransform.matrix = matrix; + graphTransform.inverseMatrix = inverseMatrix; + graphTransform.up = up; + graphTransform.translation = translation; + graphTransform.i3translation = i3translation; + graphTransform.rotation = rotation; + graphTransform.inverseRotation = inverseRotation; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs.meta new file mode 100644 index 0000000..d9d146c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: f9d3961465175430a84fd52d1bd31b05 +timeCreated: 1474479722 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs new file mode 100644 index 0000000..f412ab7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs @@ -0,0 +1,220 @@ +using System.Collections.Generic; + +namespace Pathfinding.Graphs.Util { + /// <summary> + /// Holds a lookup datastructure to quickly find objects inside rectangles. + /// Objects of type T occupy an integer rectangle in the grid and they can be + /// moved efficiently. You can query for all objects that touch a specified + /// rectangle that runs in O(m*k+r) time where m is the number of objects that + /// the query returns, k is the average number of cells that an object + /// occupies and r is the area of the rectangle query. + /// + /// All objects must be contained within a rectangle with one point at the origin + /// (inclusive) and one at <see cref="size"/> (exclusive) that is specified in the constructor. + /// </summary> + public class GridLookup<T> where T : class { + Int2 size; + Item[] cells; + /// <summary> + /// Linked list of all items. + /// Note that the first item in the list is a dummy item and does not contain any data. + /// </summary> + Root all = new Root(); + Dictionary<T, Root> rootLookup = new Dictionary<T, Root>(); + Stack<Item> itemPool = new Stack<Item>(); + + public GridLookup (Int2 size) { + this.size = size; + cells = new Item[size.x*size.y]; + for (int i = 0; i < cells.Length; i++) cells[i] = new Item(); + } + + internal class Item { + public Root root; + public Item prev, next; + } + + public class Root { + /// <summary>Underlying object</summary> + public T obj; + /// <summary>Next item in the linked list of all roots</summary> + public Root next; + /// <summary>Previous item in the linked list of all roots</summary> + internal Root prev; + internal IntRect previousBounds = new IntRect(0, 0, -1, -1); + /// <summary>References to an item in each grid cell that this object is contained inside</summary> + internal List<Item> items = new List<Item>(); + internal bool flag; + + public UnityEngine.Vector3 previousPosition = new UnityEngine.Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + public UnityEngine.Quaternion previousRotation; + } + + /// <summary>Linked list of all items</summary> + public Root AllItems { + get { + return all.next; + } + } + + public void Clear () { + rootLookup.Clear(); + all.next = null; + foreach (var item in cells) item.next = null; + } + + public Root GetRoot (T item) { + rootLookup.TryGetValue(item, out Root root); + return root; + } + + /// <summary> + /// Add an object to the lookup data structure. + /// Returns: A handle which can be used for Move operations + /// </summary> + public Root Add (T item, IntRect bounds) { + var root = new Root { + obj = item, + prev = all, + next = all.next + }; + + all.next = root; + if (root.next != null) root.next.prev = root; + + rootLookup.Add(item, root); + Move(item, bounds); + return root; + } + + /// <summary>Removes an item from the lookup data structure</summary> + public void Remove (T item) { + if (!rootLookup.TryGetValue(item, out Root root)) { + return; + } + + // Make the item occupy no cells at all + Move(item, new IntRect(0, 0, -1, -1)); + rootLookup.Remove(item); + root.prev.next = root.next; + if (root.next != null) root.next.prev = root.prev; + } + + public void Dirty (T item) { + if (!rootLookup.TryGetValue(item, out Root root)) { + return; + } + + root.previousPosition = new UnityEngine.Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + } + + /// <summary>Move an object to occupy a new set of cells</summary> + public void Move (T item, IntRect bounds) { + if (!rootLookup.TryGetValue(item, out Root root)) { + throw new System.ArgumentException("The item has not been added to this object"); + } + + var prev = root.previousBounds; + if (prev == bounds) return; + + // Remove all + for (int i = 0; i < root.items.Count; i++) { + Item ob = root.items[i]; + ob.prev.next = ob.next; + if (ob.next != null) ob.next.prev = ob.prev; + } + + root.previousBounds = bounds; + int reusedItems = 0; + for (int z = bounds.ymin; z <= bounds.ymax; z++) { + for (int x = bounds.xmin; x <= bounds.xmax; x++) { + Item ob; + if (reusedItems < root.items.Count) { + ob = root.items[reusedItems]; + } else { + ob = itemPool.Count > 0 ? itemPool.Pop() : new Item(); + ob.root = root; + root.items.Add(ob); + } + reusedItems++; + + ob.prev = cells[x + z*size.x]; + ob.next = ob.prev.next; + ob.prev.next = ob; + if (ob.next != null) ob.next.prev = ob; + } + } + + for (int i = root.items.Count-1; i >= reusedItems; i--) { + Item ob = root.items[i]; + ob.root = null; + ob.next = null; + ob.prev = null; + root.items.RemoveAt(i); + itemPool.Push(ob); + } + } + + /// <summary> + /// Returns all objects of a specific type inside the cells marked by the rectangle. + /// Note: For better memory usage, consider pooling the list using Pathfinding.Util.ListPool after you are done with it + /// </summary> + public List<U> QueryRect<U>(IntRect r) where U : class, T { + List<U> result = Pathfinding.Util.ListPool<U>.Claim(); + + // Loop through tiles and check which objects are inside them + for (int z = r.ymin; z <= r.ymax; z++) { + var zs = z*size.x; + for (int x = r.xmin; x <= r.xmax; x++) { + Item c = cells[x + zs]; + // Note, first item is a dummy, so it is ignored + while (c.next != null) { + c = c.next; + var obj = c.root.obj as U; + if (!c.root.flag && obj != null) { + c.root.flag = true; + result.Add(obj); + } + } + } + } + + // Reset flags + for (int z = r.ymin; z <= r.ymax; z++) { + var zs = z*size.x; + for (int x = r.xmin; x <= r.xmax; x++) { + Item c = cells[x + zs]; + while (c.next != null) { + c = c.next; + c.root.flag = false; + } + } + } + + return result; + } + + public void Resize (IntRect newBounds) { + var newCells = new Item[newBounds.Width * newBounds.Height]; + for (int z = 0; z < size.y; z++) { + for (int x = 0; x < size.x; x++) { + if (newBounds.Contains(x, z)) { + newCells[(x - newBounds.xmin) + (z - newBounds.ymin) * newBounds.Width] = cells[x + z*size.x]; + } + } + } + for (int i = 0; i < newCells.Length; i++) { + if (newCells[i] == null) newCells[i] = new Item(); + } + this.size = new Int2(newBounds.Width, newBounds.Height); + this.cells = newCells; + var root = this.AllItems; + var offset = new Int2(-newBounds.xmin, -newBounds.ymin); + var bounds = new IntRect(0, 0, newBounds.Width - 1, newBounds.Height - 1); + while (root != null) { + root.previousBounds = IntRect.Intersection(root.previousBounds.Offset(offset), bounds); + root = root.next; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs.meta new file mode 100644 index 0000000..2d2d1f0 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 7b09e5cbe5d4644c2b4ed9eed14cc13a +timeCreated: 1475417043 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs new file mode 100644 index 0000000..3b6ed49 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs @@ -0,0 +1,4 @@ + +// This file has been removed from the project. Since UnityPackages cannot +// delete files, only replace them, this message is left here to prevent old +// files from causing compiler errors diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs.meta new file mode 100644 index 0000000..b43caee --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: b56789f958bf1496ba91f7e2b4147166 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs new file mode 100644 index 0000000..a2f1ff9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs @@ -0,0 +1,311 @@ +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Graphs.Navmesh; +using Pathfinding.Util; +using Pathfinding.Drawing; + +namespace Pathfinding { + /// <summary> + /// Explicit mesh object for recast graphs. + /// + /// Sometimes you want to tweak the navmesh on a per-object basis. For example you might want to make some objects completely unwalkable, or you might want to special case some objects to remove them from the navmesh altogether. + /// + /// You can do this using the <see cref="RecastMeshObj"/> component. Attach it to any object you want to modify and configure the settings as you wish. + /// + /// Using the <see cref="RecastMeshObj"/> component you can: + /// + /// - Exclude an object from the graph completely. + /// - Make the surfaces of an object unwalkable. + /// - Make the surfaces of an object walkable (this is just the default behavior). + /// - Create seams in the navmesh between adjacent objects. + /// - Mark the surfaces of an object with a specific tag (see tags) (view in online documentation for working links). + /// + /// Adding this component to an object will make sure it is included in any recast graphs. + /// It will be included even if the Rasterize Meshes toggle is set to false. + /// + /// Using RecastMeshObjs instead of relying on the Rasterize Meshes option is good for several reasons. + /// - Rasterize Meshes is slow. If you are using a tiled graph and you are updating it, every time something is recalculated + /// the graph will have to search all meshes in your scene for ones to rasterize. In contrast, RecastMeshObjs are stored + /// in a tree for extremely fast lookup (O(log n + k) compared to O(n) where n is the number of meshes in your scene and k is the number of meshes + /// which should be rasterized, if you know Big-O notation). + /// - The RecastMeshObj exposes some options which can not be accessed using the Rasterize Meshes toggle. See member documentation for more info. + /// This can for example be used to include meshes in the recast graph rasterization, but make sure that the character cannot walk on them. + /// + /// Since the objects are stored in a tree, and trees are slow to update, there is an enforcement that objects are not allowed to move + /// unless the <see cref="dynamic"/> option is enabled. When the dynamic option is enabled, the object will be stored in an array instead of in the tree. + /// This will reduce the performance improvement over 'Rasterize Meshes' but is still faster. + /// + /// If a mesh filter and a mesh renderer is attached to this GameObject, those will be used in the rasterization + /// otherwise if a collider is attached, that will be used. + /// </summary> + [AddComponentMenu("Pathfinding/Navmesh/RecastMeshObj")] + [DisallowMultipleComponent] + [HelpURL("https://arongranberg.com/astar/documentation/stable/recastmeshobj.html")] + public class RecastMeshObj : VersionedMonoBehaviour { + /// <summary>Components are stored in a tree for fast lookups</summary> + protected static AABBTree<RecastMeshObj> tree = new AABBTree<RecastMeshObj>(); + + /// <summary> + /// Enable if the object will move during runtime. + /// + /// If disabled, the object will be assumed to stay in the same position, and keep the same size, until the component is disabled or destroyed. + /// + /// Disabling this will provide a small performance boost when doing graph updates, + /// as the graph no longer has to check if this RecastMeshObj might have moved. + /// + /// Even you set dynamic=false, you can disable the component, move the object, and enable it at the new position. + /// </summary> + public bool dynamic = true; + + /// <summary> + /// If true then the mesh will be treated as solid and its interior will be unwalkable. + /// The unwalkable region will be the minimum to maximum y coordinate in each cell. + /// + /// If you enable this on a mesh that is actually hollow then the hollow region will also be treated as unwalkable. + /// </summary> + public bool solid = false; + + /// <summary>Source of geometry when voxelizing this object</summary> + public GeometrySource geometrySource = GeometrySource.Auto; + + /// <summary> + /// Determines if the object should be included in scans or not. + /// See: <see cref="ScanInclusion"/> + /// </summary> + public ScanInclusion includeInScan = ScanInclusion.Auto; + + public enum ScanInclusion { + /// <summary> + /// Includes or excludes the object as normal based on the recast graph's layer mask and tag mask. + /// + /// See: <see cref="RecastGraph.mask"/> + /// </summary> + Auto, + /// <summary>This object will be completely ignored by the graph</summary> + AlwaysExclude, + /// <summary>This object will always be included when scanning a recast graph, even if it would normally be filtered out</summary> + AlwaysInclude, + } + + /// <summary>Source of geometry when voxelizing this object</summary> + public enum GeometrySource { + /// <summary>Uses the MeshFilter component on this GameObject if available, otherwise uses the collider</summary> + Auto, + /// <summary>Always uses the MeshFilter component on this GameObject</summary> + MeshFilter, + /// <summary>Always uses the Collider on this GameObject</summary> + Collider, + } + + public enum Mode { + /// <summary>All surfaces on this mesh will be made unwalkable</summary> + UnwalkableSurface = 1, + /// <summary>All surfaces on this mesh will be walkable</summary> + WalkableSurface, + /// <summary>All surfaces on this mesh will be walkable and a seam will be created between the surfaces on this mesh and the surfaces on other meshes (with a different surface id)</summary> + WalkableSurfaceWithSeam, + /// <summary>All surfaces on this mesh will be walkable and the nodes will be given the specified tag. A seam will be created between the surfaces on this mesh and the surfaces on other meshes (with a different tag or surface id)</summary> + WalkableSurfaceWithTag, + } + + /// <summary> + /// Voxel area for mesh. + /// This area (not to be confused with pathfinding areas, this is only used when rasterizing meshes for the recast graph) field + /// can be used to explicitly insert edges in the navmesh geometry or to make some parts of the mesh unwalkable. + /// + /// When rasterizing the world and two objects with different surface id values are adjacent to each other, a split in the navmesh geometry + /// will be added between them, characters will still be able to walk between them, but this can be useful when working with navmesh updates. + /// + /// Navmesh updates which recalculate a whole tile (updatePhysics=True) are very slow So if there are special places + /// which you know are going to be updated quite often, for example at a door opening (opened/closed door) you + /// can use surface IDs to create splits on the navmesh for easier updating using normal graph updates (updatePhysics=False). + /// See the below video for more information. + /// + /// Video: https://www.youtube.com/watch?v=CS6UypuEMwM + /// + /// Deprecated: Use <see cref="mode"/> and <see cref="surfaceID"/> instead + /// </summary> + [System.Obsolete("Use mode and surfaceID instead")] + public int area { + get { + switch (mode) { + case Mode.UnwalkableSurface: + return -1; + case Mode.WalkableSurface: + default: + return 0; + case Mode.WalkableSurfaceWithSeam: + return surfaceID; + case Mode.WalkableSurfaceWithTag: + return surfaceID; + } + } + set { + if (value <= -1) mode = Mode.UnwalkableSurface; + if (value == 0) mode = Mode.WalkableSurface; + if (value > 0) { + mode = Mode.WalkableSurfaceWithSeam; + surfaceID = value; + } + } + } + + /// <summary> + /// Voxel area for mesh. + /// This area (not to be confused with pathfinding areas, this is only used when rasterizing meshes for the recast graph) field + /// can be used to explicitly insert edges in the navmesh geometry or to make some parts of the mesh unwalkable. + /// + /// When rasterizing the world and two objects with different surface id values are adjacent to each other, a split in the navmesh geometry + /// will be added between them, characters will still be able to walk between them, but this can be useful when working with navmesh updates. + /// + /// Navmesh updates which recalculate a whole tile (updatePhysics=True) are very slow So if there are special places + /// which you know are going to be updated quite often, for example at a door opening (opened/closed door) you + /// can use surface IDs to create splits on the navmesh for easier updating using normal graph updates (updatePhysics=False). + /// See the below video for more information. + /// + /// Video: https://www.youtube.com/watch?v=CS6UypuEMwM + /// + /// When <see cref="mode"/> is set to Mode.WalkableSurfaceWithTag then this value will be interpreted as a pathfinding tag. See tags (view in online documentation for working links). + /// + /// Note: This only has an effect if <see cref="mode"/> is set to Mode.WalkableSurfaceWithSeam or Mode.WalkableSurfaceWithTag. + /// + /// Note: Only non-negative values are valid. + /// </summary> + [UnityEngine.Serialization.FormerlySerializedAs("area")] + public int surfaceID = 1; + + /// <summary> + /// Surface rasterization mode. + /// See: <see cref="Mode"/> + /// </summary> + public Mode mode = Mode.WalkableSurface; + + AABBTree<RecastMeshObj>.Key treeKey; + + void OnEnable () { + // Clamp area, upper limit isn't really a hard limit, but if it gets much higher it will start to interfere with other stuff + surfaceID = Mathf.Clamp(surfaceID, 0, 1 << 25); + if (!treeKey.isValid) { + treeKey = tree.Add(CalculateBounds(), this); + if (this.dynamic) BatchedEvents.Add(this, BatchedEvents.Event.Custom, OnUpdate); + } + } + + void OnDisable () { + BatchedEvents.Remove(this); + var originalBounds = tree.Remove(treeKey); + treeKey = default; + if (!this.dynamic) { + var newBounds = CalculateBounds(); + // When using static baching, the bounds of the object may shrink. + // In particular, if the object has been rotated, the renderer's bounds will originally use an approximation of the AABB (presumably just the original AABB, but rotated and then axis aligned again), + // but after static batching, it actually looks at the new mesh (with the rotation baked in), and can generate a more precise AABB (which may be smaller). + // Therefore we say that it's ok as long as the original bounds contain the new bounds. + // This is fine, because the tree only needs a bounding box which contains the object. If it's too big, it will just be a bit more conservative. + // Also expand the original bounding box by a tiny amount to work around floating point errors. + originalBounds.Expand(0.001f); + newBounds.Encapsulate(originalBounds); + if ((newBounds.center - originalBounds.center).sqrMagnitude > 0.01f*0.01f || (newBounds.extents - originalBounds.extents).sqrMagnitude > 0.01f*0.01f) { + Debug.LogError("The RecastMeshObj has been moved or resized since it was enabled. You should set dynamic to true for moving objects, or disable the component while moving it. The bounds changed from " + originalBounds + " to " + newBounds, this); + } + } + } + + static void OnUpdate (RecastMeshObj[] components, int _) { + for (int i = 0; i < components.Length; i++) { + var comp = components[i]; + if (comp != null && comp.transform.hasChanged) { + var bounds = comp.CalculateBounds(); + if (tree.GetBounds(comp.treeKey) != bounds) tree.Move(comp.treeKey, bounds); + comp.transform.hasChanged = false; + } + } + } + + /// <summary>Fills the buffer with all RecastMeshObjs which intersect the specified bounds</summary> + public static void GetAllInBounds (List<RecastMeshObj> buffer, Bounds bounds) { + // Refreshes the tree if necessary + BatchedEvents.ProcessEvent<RecastMeshObj>(BatchedEvents.Event.Custom); + + if (!Application.isPlaying) { + var objs = UnityCompatibility.FindObjectsByTypeSorted<RecastMeshObj>(); + for (int i = 0; i < objs.Length; i++) { + if (objs[i].enabled) { + if (bounds.Intersects(objs[i].CalculateBounds())) { + buffer.Add(objs[i]); + } + } + } + return; + } else if (Time.timeSinceLevelLoad == 0) { + // Is is not guaranteed that all RecastMeshObj OnEnable functions have been called, so if it is the first frame since loading a new level + // try to initialize all RecastMeshObj objects. + var objs = UnityCompatibility.FindObjectsByTypeUnsorted<RecastMeshObj>(); + for (int i = 0; i < objs.Length; i++) objs[i].OnEnable(); + } + + tree.Query(bounds, buffer); + } + + /// <summary> + /// Resolves the geometry source that is to be used. + /// Will output either a MeshFilter, a Collider, or a 2D collider, never more than one. + /// If all are null, then no geometry could be found. + /// + /// See: <see cref="geometrySource"/> + /// </summary> + public void ResolveMeshSource (out MeshFilter meshFilter, out Collider collider, out Collider2D collider2D) { + meshFilter = null; + collider = null; + collider2D = null; + switch (geometrySource) { + case GeometrySource.Auto: + if (TryGetComponent<MeshRenderer>(out _) && TryGetComponent<MeshFilter>(out meshFilter) && meshFilter.sharedMesh != null) return; + if (TryGetComponent<Collider>(out collider)) return; + TryGetComponent<Collider2D>(out collider2D); + break; + case GeometrySource.MeshFilter: + TryGetComponent<MeshFilter>(out meshFilter); + break; + case GeometrySource.Collider: + if (TryGetComponent<Collider>(out collider)) return; + TryGetComponent<Collider2D>(out collider2D); + break; + default: + throw new System.ArgumentOutOfRangeException(); + } + } + + /// <summary>Calculates and returns the bounding box containing all geometry to be rasterized</summary> + private Bounds CalculateBounds () { + ResolveMeshSource(out var filter, out var coll, out var coll2D); + + if (coll != null) { + return coll.bounds; + } else if (coll2D != null) { + return coll2D.bounds; + } else if (filter != null) { + if (TryGetComponent<MeshRenderer>(out var rend)) { + return rend.bounds; + } else { + Debug.LogError("Cannot use a MeshFilter as a geomtry source without a MeshRenderer attached to the same GameObject.", this); + return new Bounds(Vector3.zero, Vector3.one); + } + } else { + Debug.LogError("Could not find an attached mesh source", this); + return new Bounds(Vector3.zero, Vector3.one); + } + } + + protected override void OnUpgradeSerializedData (ref Serialization.Migrations migrations, bool unityThread) { + if (migrations.TryMigrateFromLegacyFormat(out var legacyVersion)) { + #pragma warning disable 618 + if (legacyVersion == 1) area = surfaceID; + #pragma warning restore 618 + if (legacyVersion <= 2) includeInScan = ScanInclusion.AlwaysInclude; + // Mode.ExcludeFromGraph was changed to ScanInclusion.AlwaysExclude + if (mode == (Mode)0) includeInScan = ScanInclusion.AlwaysExclude; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs.meta new file mode 100644 index 0000000..5ef9823 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 220345cbaa167417bbe806f230f68615 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs new file mode 100644 index 0000000..03565bd --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs @@ -0,0 +1,341 @@ +namespace Pathfinding.Jobs { + using UnityEngine; + using Unity.Burst; + using Unity.Collections; + using Unity.Jobs; + using Unity.Mathematics; + using UnityEngine.Assertions; + using Pathfinding.Graphs.Grid; + using Pathfinding.Util; + + /// <summary> + /// Slice of a 3D array. + /// + /// This is a helper struct used in many jobs to make them work on a part of the data. + /// + /// The outer array has the size <see cref="outerSize"/>.x * <see cref="outerSize"/>.y * <see cref="outerSize"/>.z, laid out as if the coordinates were sorted by the tuple (Y,Z,X). + /// The inner array has the size <see cref="slice.size"/>.x * <see cref="slice.size"/>.y * <see cref="slice.size"/>.z, also laid out as if the coordinates were sorted by the tuple (Y,Z,X). + /// </summary> + public readonly struct Slice3D { + public readonly int3 outerSize; + public readonly IntBounds slice; + + public Slice3D (IntBounds outer, IntBounds slice) : this(outer.size, slice.Offset(-outer.min)) {} + + + public Slice3D (int3 outerSize, IntBounds slice) { + this.outerSize = outerSize; + this.slice = slice; + Assert.IsTrue(slice.min.x >= 0 && slice.min.y >= 0 && slice.min.z >= 0); + Assert.IsTrue(slice.max.x <= outerSize.x && slice.max.y <= outerSize.y && slice.max.z <= outerSize.z); + Assert.IsTrue(slice.size.x > 0 && slice.size.y > 0 && slice.size.z > 0); + } + + public void AssertMatchesOuter<T>(UnsafeSpan<T> values) where T : unmanaged { + Assert.AreEqual(outerSize.x * outerSize.y * outerSize.z, values.Length); + } + + public void AssertMatchesOuter<T>(NativeArray<T> values) where T : struct { + Assert.AreEqual(outerSize.x * outerSize.y * outerSize.z, values.Length); + } + + public void AssertMatchesInner<T>(NativeArray<T> values) where T : struct { + Assert.AreEqual(slice.size.x * slice.size.y * slice.size.z, values.Length); + } + + public void AssertSameSize (Slice3D other) { + Assert.AreEqual(slice.size, other.slice.size); + } + + public int InnerCoordinateToOuterIndex (int x, int y, int z) { + var(dx, dy, dz) = outerStrides; + return (x + slice.min.x) * dx + (y + slice.min.y) * dy + (z + slice.min.z) * dz; + } + + public int length => slice.size.x * slice.size.y * slice.size.z; + + public (int, int, int)outerStrides => (1, outerSize.x * outerSize.z, outerSize.x); + public (int, int, int)innerStrides => (1, slice.size.x * slice.size.z, slice.size.x); + public int outerStartIndex { + get { + var(dx, dy, dz) = outerStrides; + return slice.min.x * dx + slice.min.y * dy + slice.min.z * dz; + } + } + + /// <summary>True if the slice covers the whole outer array</summary> + public bool coversEverything => math.all(slice.size == outerSize); + } + + /// <summary>Helpers for scheduling simple NativeArray jobs</summary> + static class NativeArrayExtensions { + /// <summary>this[i] = value</summary> + public static JobMemSet<T> MemSet<T>(this NativeArray<T> self, T value) where T : unmanaged { + return new JobMemSet<T> { + data = self, + value = value, + }; + } + + /// <summary>this[i] &= other[i]</summary> + public static JobAND BitwiseAndWith (this NativeArray<bool> self, NativeArray<bool> other) { + return new JobAND { + result = self, + data = other, + }; + } + + /// <summary>to[i] = from[i]</summary> + public static JobCopy<T> CopyToJob<T>(this NativeArray<T> from, NativeArray<T> to) where T : struct { + return new JobCopy<T> { + from = from, + to = to, + }; + } + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public static SliceActionJob<T> WithSlice<T>(this T action, Slice3D slice) where T : struct, GridIterationUtilities.ISliceAction { + return new SliceActionJob<T> { + action = action, + slice = slice, + }; + } + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public static IndexActionJob<T> WithLength<T>(this T action, int length) where T : struct, GridIterationUtilities.ISliceAction { + return new IndexActionJob<T> { + action = action, + length = length, + }; + } + + public static JobRotate3DArray<T> Rotate3D<T>(this NativeArray<T> arr, int3 size, int dx, int dz) where T : unmanaged { + return new JobRotate3DArray<T> { + arr = arr, + size = size, + dx = dx, + dz = dz, + }; + } + } + + /// <summary> + /// Treats input as a 3-dimensional array and copies it into the output at the specified position. + /// + /// The <see cref="input"/> is a 3D array, and <see cref="inputSlice"/> refers to a rectangular slice of this array. + /// The <see cref="output"/> is defined similarly. + /// + /// The two slices must naturally have the same shape. + /// </summary> + [BurstCompile] + public struct JobCopyRectangle<T> : IJob where T : struct { + [ReadOnly] + [DisableUninitializedReadCheck] // TODO: Fix so that job doesn't run instead + public NativeArray<T> input; + + [WriteOnly] + public NativeArray<T> output; + + public Slice3D inputSlice; + public Slice3D outputSlice; + + public void Execute () { + Copy(input, output, inputSlice, outputSlice); + } + + /// <summary> + /// Treats input as a 3-dimensional array and copies it into the output at the specified position. + /// + /// The input is a 3D array, and inputSlice refers to a rectangular slice of this array. + /// The output is defined similarly. + /// + /// The two slices must naturally have the same shape. + /// </summary> + public static void Copy (NativeArray<T> input, NativeArray<T> output, Slice3D inputSlice, Slice3D outputSlice) { + inputSlice.AssertMatchesOuter(input); + outputSlice.AssertMatchesOuter(output); + inputSlice.AssertSameSize(outputSlice); + + if (inputSlice.coversEverything && outputSlice.coversEverything) { + // One contiguous chunk + // TODO: Check can be made better by only checking if it is a contiguous chunk instead of covering the whole arrays + input.CopyTo(output); + } else { + // Copy row-by-row + for (int y = 0; y < outputSlice.slice.size.y; y++) { + for (int z = 0; z < outputSlice.slice.size.z; z++) { + var rowOffsetInput = inputSlice.InnerCoordinateToOuterIndex(0, y, z); + var rowOffsetOutput = outputSlice.InnerCoordinateToOuterIndex(0, y, z); + // Using a raw MemCpy call is a bit faster, but that requires unsafe code + // Using a for loop is *a lot* slower (except for very small arrays, in which case it is about the same or very slightly faster). + NativeArray<T>.Copy(input, rowOffsetInput, output, rowOffsetOutput, outputSlice.slice.size.x); + } + } + } + } + } + + /// <summary>result[i] = value</summary> + [BurstCompile] + public struct JobMemSet<T> : IJob where T : unmanaged { + [WriteOnly] + public NativeArray<T> data; + + public T value; + + public void Execute() => data.AsUnsafeSpan().Fill(value); + } + + /// <summary>to[i] = from[i]</summary> + [BurstCompile] + public struct JobCopy<T> : IJob where T : struct { + [ReadOnly] + public NativeArray<T> from; + + [WriteOnly] + public NativeArray<T> to; + + public void Execute () { + from.CopyTo(to); + } + } + + [BurstCompile] + public struct IndexActionJob<T> : IJob where T : struct, GridIterationUtilities.ISliceAction { + public T action; + public int length; + + public void Execute () { + for (int i = 0; i < length; i++) action.Execute((uint)i, (uint)i); + } + } + + [BurstCompile] + public struct SliceActionJob<T> : IJob where T : struct, GridIterationUtilities.ISliceAction { + public T action; + public Slice3D slice; + + public void Execute () { + GridIterationUtilities.ForEachCellIn3DSlice(slice, ref action); + } + } + + /// <summary>result[i] &= data[i]</summary> + public struct JobAND : GridIterationUtilities.ISliceAction { + public NativeArray<bool> result; + + [ReadOnly] + public NativeArray<bool> data; + + public void Execute (uint outerIdx, uint innerIdx) { + result[(int)outerIdx] &= data[(int)outerIdx]; + } + } + + [BurstCompile] + public struct JobMaxHitCount : IJob { + [ReadOnly] + public NativeArray<RaycastHit> hits; + public int maxHits; + public int layerStride; + [WriteOnly] + public NativeArray<int> maxHitCount; + public void Execute () { + int maxHit = 0; + + for (; maxHit < maxHits; maxHit++) { + int offset = maxHit * layerStride; + bool any = false; + for (int i = offset; i < offset + layerStride; i++) { + if (math.any(hits[i].normal)) { + any = true; + break; + } + } + + if (!any) break; + } + + maxHitCount[0] = math.max(1, maxHit); + } + } + + /// <summary> + /// Copies hit points and normals. + /// points[i] = hits[i].point (if anything was hit), normals[i] = hits[i].normal.normalized. + /// </summary> + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobCopyHits : IJob, GridIterationUtilities.ISliceAction { + [ReadOnly] + public NativeArray<RaycastHit> hits; + + [WriteOnly] + public NativeArray<Vector3> points; + + [WriteOnly] + public NativeArray<float4> normals; + public Slice3D slice; + + public void Execute () { + // The number of hits may be larger than the number of points. The remaining hits are not actually hits. + Assert.IsTrue(hits.Length >= slice.length); + slice.AssertMatchesOuter(points); + slice.AssertMatchesOuter(normals); + GridIterationUtilities.ForEachCellIn3DSlice(slice, ref this); + } + + public void Execute (uint outerIdx, uint innerIdx) { + Unity.Burst.CompilerServices.Aliasing.ExpectNotAliased(points, normals); + var normal = hits[(int)innerIdx].normal; + var normalV4 = new float4(normal.x, normal.y, normal.z, 0); + normals[(int)outerIdx] = math.normalizesafe(normalV4); + + // Check if anything was hit. The normal will be zero otherwise + // If nothing was hit then the existing data in the points array is reused + if (math.lengthsq(normalV4) > math.FLT_MIN_NORMAL) { + points[(int)outerIdx] = hits[(int)innerIdx].point; + } + } + } + + [BurstCompile] + public struct JobRotate3DArray<T>: IJob where T : unmanaged { + public NativeArray<T> arr; + public int3 size; + public int dx, dz; + + public void Execute () { + int width = size.x; + int height = size.y; + int depth = size.z; + var span = arr.AsUnsafeSpan(); + dx = dx % width; + dz = dz % depth; + if (dx != 0) { + if (dx < 0) dx = width + dx; + var tmp = new NativeArray<T>(dx, Allocator.Temp); + var tmpSpan = tmp.AsUnsafeSpan(); + for (int y = 0; y < height; y++) { + var offset = y * width * depth; + for (int z = 0; z < depth; z++) { + span.Slice(offset + z * width + width - dx, dx).CopyTo(tmpSpan); + span.Move(offset + z * width, offset + z * width + dx, width - dx); + tmpSpan.CopyTo(span.Slice(offset + z * width, dx)); + } + } + } + + if (dz != 0) { + if (dz < 0) dz = depth + dz; + var tmp = new NativeArray<T>(dz * width, Allocator.Temp); + var tmpSpan = tmp.AsUnsafeSpan(); + for (int y = 0; y < height; y++) { + var offset = y * width * depth; + span.Slice(offset + (depth - dz) * width, dz * width).CopyTo(tmpSpan); + span.Move(offset, offset + dz * width, (depth - dz) * width); + tmpSpan.CopyTo(span.Slice(offset, dz * width)); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs.meta new file mode 100644 index 0000000..aa515bf --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c3924c162ff1c47bf914318910ba0a43 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: |