diff options
Diffstat (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid')
54 files changed, 3746 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs new file mode 100644 index 0000000..a21f179 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs @@ -0,0 +1,387 @@ +using UnityEngine; +using System.Collections.Generic; +using Unity.Collections; + +namespace Pathfinding.Graphs.Grid { + using Pathfinding.Util; + using Pathfinding.Graphs.Grid.Jobs; + using Pathfinding.Jobs; + + /// <summary> + /// Handles collision checking for graphs. + /// Mostly used by grid based graphs + /// </summary> + [System.Serializable] + public class GraphCollision { + /// <summary> + /// Collision shape to use. + /// See: <see cref="ColliderType"/> + /// </summary> + public ColliderType type = ColliderType.Capsule; + + /// <summary> + /// Diameter of capsule or sphere when checking for collision. + /// When checking for collisions the system will check if any colliders + /// overlap a specific shape at the node's position. The shape is determined + /// by the <see cref="type"/> field. + /// + /// A diameter of 1 means that the shape has a diameter equal to the node's width, + /// or in other words it is equal to <see cref="Pathfinding.GridGraph.nodeSize"/>. + /// + /// If <see cref="type"/> is set to Ray, this does not affect anything. + /// + /// [Open online documentation to see images] + /// </summary> + public float diameter = 1F; + + /// <summary> + /// Height of capsule or length of ray when checking for collision. + /// If <see cref="type"/> is set to Sphere, this does not affect anything. + /// + /// [Open online documentation to see images] + /// + /// Warning: In contrast to Unity's capsule collider and character controller this height does not include the end spheres of the capsule, but only the cylinder part. + /// This is mostly for historical reasons. + /// </summary> + public float height = 2F; + + /// <summary> + /// Height above the ground that collision checks should be done. + /// For example, if the ground was found at y=0, collisionOffset = 2 + /// type = Capsule and height = 3 then the physics system + /// will be queried to see if there are any colliders in a capsule + /// for which the bottom sphere that is made up of is centered at y=2 + /// and the top sphere has its center at y=2+3=5. + /// + /// If type = Sphere then the sphere's center would be at y=2 in this case. + /// </summary> + public float collisionOffset; + + /// <summary> + /// Direction of the ray when checking for collision. + /// If <see cref="type"/> is not Ray, this does not affect anything + /// + /// Deprecated: Only the Both mode is supported now. + /// </summary> + [System.Obsolete("Only the Both mode is supported now")] + public RayDirection rayDirection = RayDirection.Both; + + /// <summary>Layers to be treated as obstacles.</summary> + public LayerMask mask; + + /// <summary>Layers to be included in the height check.</summary> + public LayerMask heightMask = -1; + + /// <summary> + /// The height to check from when checking height ('ray length' in the inspector). + /// + /// As the image below visualizes, different ray lengths can make the ray hit different things. + /// The distance is measured up from the graph plane. + /// + /// [Open online documentation to see images] + /// </summary> + public float fromHeight = 100; + + /// <summary> + /// Toggles thick raycast. + /// See: https://docs.unity3d.com/ScriptReference/Physics.SphereCast.html + /// </summary> + public bool thickRaycast; + + /// <summary> + /// Diameter of the thick raycast in nodes. + /// 1 equals <see cref="Pathfinding.GridGraph.nodeSize"/> + /// </summary> + public float thickRaycastDiameter = 1; + + /// <summary>Make nodes unwalkable when no ground was found with the height raycast. If height raycast is turned off, this doesn't affect anything.</summary> + public bool unwalkableWhenNoGround = true; + + /// <summary> + /// Use Unity 2D Physics API. + /// + /// If enabled, the 2D Physics API will be used, and if disabled, the 3D Physics API will be used. + /// + /// This changes the collider types (see <see cref="type)"/> from 3D versions to their corresponding 2D versions. For example the sphere shape becomes a circle. + /// + /// The <see cref="heightCheck"/> setting will be ignored when 2D physics is used. + /// + /// See: http://docs.unity3d.com/ScriptReference/Physics2D.html + /// </summary> + public bool use2D; + + /// <summary>Toggle collision check</summary> + public bool collisionCheck = true; + + /// <summary> + /// Toggle height check. If false, the grid will be flat. + /// + /// This setting will be ignored when 2D physics is used. + /// </summary> + public bool heightCheck = true; + + /// <summary> + /// Direction to use as UP. + /// See: Initialize + /// </summary> + public Vector3 up; + + /// <summary> + /// <see cref="up"/> * <see cref="height"/>. + /// See: Initialize + /// </summary> + private Vector3 upheight; + + /// <summary>Used for 2D collision queries</summary> + private ContactFilter2D contactFilter; + + /// <summary> + /// Just so that the Physics2D.OverlapPoint method has some buffer to store things in. + /// We never actually read from this array, so we don't even care if this is thread safe. + /// </summary> + private static Collider2D[] dummyArray = new Collider2D[1]; + + /// <summary> + /// <see cref="diameter"/> * scale * 0.5. + /// Where scale usually is <see cref="Pathfinding.GridGraph.nodeSize"/> + /// See: Initialize + /// </summary> + private float finalRadius; + + /// <summary> + /// <see cref="thickRaycastDiameter"/> * scale * 0.5. + /// Where scale usually is <see cref="Pathfinding.GridGraph.nodeSize"/> See: Initialize + /// </summary> + private float finalRaycastRadius; + + /// <summary>Offset to apply after each raycast to make sure we don't hit the same point again in CheckHeightAll</summary> + public const float RaycastErrorMargin = 0.005F; + + /// <summary> + /// Sets up several variables using the specified matrix and scale. + /// See: GraphCollision.up + /// See: GraphCollision.upheight + /// See: GraphCollision.finalRadius + /// See: GraphCollision.finalRaycastRadius + /// </summary> + public void Initialize (GraphTransform transform, float scale) { + up = (transform.Transform(Vector3.up) - transform.Transform(Vector3.zero)).normalized; + upheight = up*height; + finalRadius = diameter*scale*0.5F; + finalRaycastRadius = thickRaycastDiameter*scale*0.5F; + contactFilter = new ContactFilter2D { layerMask = mask, useDepth = false, useLayerMask = true, useNormalAngle = false, useTriggers = false }; + } + + /// <summary> + /// Returns true if the position is not obstructed. + /// If <see cref="collisionCheck"/> is false, this will always return true. + /// </summary> + public bool Check (Vector3 position) { + if (!collisionCheck) { + return true; + } + + if (use2D) { + switch (type) { + case ColliderType.Capsule: + case ColliderType.Sphere: + return Physics2D.OverlapCircle(position, finalRadius, contactFilter, dummyArray) == 0; + default: + return Physics2D.OverlapPoint(position, contactFilter, dummyArray) == 0; + } + } + + position += up*collisionOffset; + switch (type) { + case ColliderType.Capsule: + return !Physics.CheckCapsule(position, position+upheight, finalRadius, mask, QueryTriggerInteraction.Ignore); + case ColliderType.Sphere: + return !Physics.CheckSphere(position, finalRadius, mask, QueryTriggerInteraction.Ignore); + default: + return !Physics.Raycast(position, up, height, mask, QueryTriggerInteraction.Ignore) && !Physics.Raycast(position+upheight, -up, height, mask, QueryTriggerInteraction.Ignore); + } + } + + /// <summary> + /// Returns the position with the correct height. + /// If <see cref="heightCheck"/> is false, this will return position. + /// </summary> + public Vector3 CheckHeight (Vector3 position) { + RaycastHit hit; + bool walkable; + + return CheckHeight(position, out hit, out walkable); + } + + /// <summary> + /// Returns the position with the correct height. + /// If <see cref="heightCheck"/> is false, this will return position. + /// walkable will be set to false if nothing was hit. + /// The ray will check a tiny bit further than to the grids base to avoid floating point errors when the ground is exactly at the base of the grid + /// </summary> + public Vector3 CheckHeight (Vector3 position, out RaycastHit hit, out bool walkable) { + walkable = true; + + if (!heightCheck || use2D) { + hit = new RaycastHit(); + return position; + } + + if (thickRaycast) { + var ray = new Ray(position+up*fromHeight, -up); + if (Physics.SphereCast(ray, finalRaycastRadius, out hit, fromHeight+0.005F, heightMask, QueryTriggerInteraction.Ignore)) { + return VectorMath.ClosestPointOnLine(ray.origin, ray.origin+ray.direction, hit.point); + } + + walkable &= !unwalkableWhenNoGround; + } else { + // Cast a ray from above downwards to try to find the ground + if (Physics.Raycast(position+up*fromHeight, -up, out hit, fromHeight+0.005F, heightMask, QueryTriggerInteraction.Ignore)) { + return hit.point; + } + + walkable &= !unwalkableWhenNoGround; + } + return position; + } + + /// <summary>Internal buffer used by <see cref="CheckHeightAll"/></summary> + RaycastHit[] hitBuffer = new RaycastHit[8]; + + /// <summary> + /// Returns all hits when checking height for position. + /// Warning: Does not work well with thick raycast, will only return an object a single time + /// + /// Warning: The returned array is ephermal. It will be invalidated when this method is called again. + /// If you need persistent results you should copy it. + /// + /// The returned array may be larger than the actual number of hits, the numHits out parameter indicates how many hits there actually were. + /// </summary> + public RaycastHit[] CheckHeightAll (Vector3 position, out int numHits) { + if (!heightCheck || use2D) { + hitBuffer[0] = new RaycastHit { + point = position, + distance = 0, + }; + numHits = 1; + return hitBuffer; + } + + // Cast a ray from above downwards to try to find the ground +#if UNITY_2017_1_OR_NEWER + numHits = Physics.RaycastNonAlloc(position+up*fromHeight, -up, hitBuffer, fromHeight+0.005F, heightMask, QueryTriggerInteraction.Ignore); + if (numHits == hitBuffer.Length) { + // Try again with a larger buffer + hitBuffer = new RaycastHit[hitBuffer.Length*2]; + return CheckHeightAll(position, out numHits); + } + return hitBuffer; +#else + var result = Physics.RaycastAll(position+up*fromHeight, -up, fromHeight+0.005F, heightMask, QueryTriggerInteraction.Ignore); + numHits = result.Length; + return result; +#endif + } + + /// <summary> + /// Returns if the position is obstructed for all nodes using the Ray collision checking method. + /// collisionCheckResult[i] = true if there were no obstructions, false otherwise + /// </summary> + public void JobCollisionRay (NativeArray<Vector3> nodePositions, NativeArray<bool> collisionCheckResult, Vector3 up, Allocator allocationMethod, JobDependencyTracker dependencyTracker) { + var collisionRaycastCommands1 = dependencyTracker.NewNativeArray<RaycastCommand>(nodePositions.Length, allocationMethod); + var collisionRaycastCommands2 = dependencyTracker.NewNativeArray<RaycastCommand>(nodePositions.Length, allocationMethod); + var collisionHits1 = dependencyTracker.NewNativeArray<RaycastHit>(nodePositions.Length, allocationMethod); + var collisionHits2 = dependencyTracker.NewNativeArray<RaycastHit>(nodePositions.Length, allocationMethod); + + // Fire rays from above down to the nodes' positions + new JobPrepareRaycasts { + origins = nodePositions, + originOffset = up * (height + collisionOffset), + direction = -up, + distance = height, + mask = mask, + physicsScene = Physics.defaultPhysicsScene, + raycastCommands = collisionRaycastCommands1, + }.Schedule(dependencyTracker); + + // Fire rays from the node up towards the sky + new JobPrepareRaycasts { + origins = nodePositions, + originOffset = up * collisionOffset, + direction = up, + distance = height, + mask = mask, + physicsScene = Physics.defaultPhysicsScene, + raycastCommands = collisionRaycastCommands2, + }.Schedule(dependencyTracker); + + dependencyTracker.ScheduleBatch(collisionRaycastCommands1, collisionHits1, 2048); + dependencyTracker.ScheduleBatch(collisionRaycastCommands2, collisionHits2, 2048); + + new JobMergeRaycastCollisionHits { + hit1 = collisionHits1, + hit2 = collisionHits2, + result = collisionCheckResult, + }.Schedule(dependencyTracker); + } + +#if UNITY_2022_2_OR_NEWER + public void JobCollisionCapsule (NativeArray<Vector3> nodePositions, NativeArray<bool> collisionCheckResult, Vector3 up, Allocator allocationMethod, JobDependencyTracker dependencyTracker) { + var commands = dependencyTracker.NewNativeArray<OverlapCapsuleCommand>(nodePositions.Length, allocationMethod); + var collisionHits = dependencyTracker.NewNativeArray<ColliderHit>(nodePositions.Length, allocationMethod); + new JobPrepareCapsuleCommands { + origins = nodePositions, + originOffset = up * collisionOffset, + direction = up * height, + radius = finalRadius, + mask = mask, + commands = commands, + physicsScene = Physics.defaultPhysicsScene, + }.Schedule(dependencyTracker); + dependencyTracker.ScheduleBatch(commands, collisionHits, 2048); + new JobColliderHitsToBooleans { + hits = collisionHits, + result = collisionCheckResult, + }.Schedule(dependencyTracker); + } + + public void JobCollisionSphere (NativeArray<Vector3> nodePositions, NativeArray<bool> collisionCheckResult, Vector3 up, Allocator allocationMethod, JobDependencyTracker dependencyTracker) { + var commands = dependencyTracker.NewNativeArray<OverlapSphereCommand>(nodePositions.Length, allocationMethod); + var collisionHits = dependencyTracker.NewNativeArray<ColliderHit>(nodePositions.Length, allocationMethod); + new JobPrepareSphereCommands { + origins = nodePositions, + originOffset = up * collisionOffset, + radius = finalRadius, + mask = mask, + commands = commands, + physicsScene = Physics.defaultPhysicsScene, + }.Schedule(dependencyTracker); + dependencyTracker.ScheduleBatch(commands, collisionHits, 2048); + new JobColliderHitsToBooleans { + hits = collisionHits, + result = collisionCheckResult, + }.Schedule(dependencyTracker); + } +#endif + } + + /// <summary> + /// Determines collision check shape. + /// See: <see cref="GraphCollision"/> + /// </summary> + public enum ColliderType { + /// <summary>Uses a Sphere, Physics.CheckSphere. In 2D this is a circle instead.</summary> + Sphere, + /// <summary>Uses a Capsule, Physics.CheckCapsule. This will behave identically to the Sphere mode in 2D.</summary> + Capsule, + /// <summary>Uses a Ray, Physics.Linecast. In 2D this is a single point instead.</summary> + Ray + } + + /// <summary>Determines collision check ray direction</summary> + public enum RayDirection { + Up, /// <summary>< Casts the ray from the bottom upwards</summary> + Down, /// <summary>< Casts the ray from the top downwards</summary> + Both /// <summary>< Casts two rays in both directions</summary> + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs.meta new file mode 100644 index 0000000..373fbfc --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b75c44e59ef4c7545af071adedd2ee03 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs new file mode 100644 index 0000000..bba0a60 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs @@ -0,0 +1,37 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Grid { + public interface GridAdjacencyMapper { + int LayerCount(IntBounds bounds); + int GetNeighbourIndex(int nodeIndexXZ, int nodeIndex, int direction, NativeArray<ulong> nodeConnections, NativeArray<int> neighbourOffsets, int layerStride); + bool HasConnection(int nodeIndex, int direction, NativeArray<ulong> nodeConnections); + } + + public struct FlatGridAdjacencyMapper : GridAdjacencyMapper { + public int LayerCount (IntBounds bounds) { + UnityEngine.Assertions.Assert.IsTrue(bounds.size.y == 1); + return 1; + } + public int GetNeighbourIndex (int nodeIndexXZ, int nodeIndex, int direction, NativeArray<ulong> nodeConnections, NativeArray<int> neighbourOffsets, int layerStride) { + return nodeIndex + neighbourOffsets[direction]; + } + public bool HasConnection (int nodeIndex, int direction, NativeArray<ulong> nodeConnections) { + return ((nodeConnections[nodeIndex] >> direction) & 0x1) != 0; + } + } + + public struct LayeredGridAdjacencyMapper : GridAdjacencyMapper { + public int LayerCount(IntBounds bounds) => bounds.size.y; + public int GetNeighbourIndex (int nodeIndexXZ, int nodeIndex, int direction, NativeArray<ulong> nodeConnections, NativeArray<int> neighbourOffsets, int layerStride) { + return nodeIndexXZ + neighbourOffsets[direction] + (int)((nodeConnections[nodeIndex] >> LevelGridNode.ConnectionStride*direction) & LevelGridNode.ConnectionMask) * layerStride; + } + public bool HasConnection (int nodeIndex, int direction, NativeArray<ulong> nodeConnections) { + return ((nodeConnections[nodeIndex] >> LevelGridNode.ConnectionStride*direction) & LevelGridNode.ConnectionMask) != LevelGridNode.NoConnection; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs.meta new file mode 100644 index 0000000..053f5a7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6c5c520c833b3b444b17fcb264c08a7a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs new file mode 100644 index 0000000..e1a4fbb --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs @@ -0,0 +1,700 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Util; +using UnityEngine.Profiling; +using System.Collections.Generic; +using Pathfinding.Jobs; +using Pathfinding.Graphs.Grid.Jobs; +using Unity.Jobs.LowLevel.Unsafe; + +namespace Pathfinding.Graphs.Grid { + public struct GridGraphNodeData { + public Allocator allocationMethod; + public int numNodes; + /// <summary> + /// Bounds for the part of the graph that this data represents. + /// For example if the first layer of a layered grid graph is being updated between x=10 and x=20, z=5 and z=15 + /// then this will be IntBounds(xmin=10, ymin=0, zmin=5, xmax=20, ymax=0, zmax=15) + /// </summary> + public IntBounds bounds; + /// <summary> + /// Number of layers that the data contains. + /// For a non-layered grid graph this will always be 1. + /// </summary> + public int layers => bounds.size.y; + + /// <summary> + /// Positions of all nodes. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid + /// - BeforeConnections: Valid + /// - AfterConnections: Valid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// </summary> + public NativeArray<Vector3> positions; + + /// <summary> + /// Bitpacked connections of all nodes. + /// + /// Connections are stored in different formats depending on <see cref="layeredDataLayout"/>. + /// You can use <see cref="LayeredGridAdjacencyMapper"/> and <see cref="FlatGridAdjacencyMapper"/> to access connections for the different data layouts. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Invalid + /// - BeforeConnections: Invalid + /// - AfterConnections: Valid + /// - AfterErosion: Valid (but will be overwritten) + /// - PostProcess: Valid + /// </summary> + public NativeArray<ulong> connections; + + /// <summary> + /// Bitpacked connections of all nodes. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid + /// - BeforeConnections: Valid + /// - AfterConnections: Valid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// </summary> + public NativeArray<uint> penalties; + + /// <summary> + /// Tags of all nodes + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid (but if erosion uses tags then it will be overwritten later) + /// - BeforeConnections: Valid (but if erosion uses tags then it will be overwritten later) + /// - AfterConnections: Valid (but if erosion uses tags then it will be overwritten later) + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// </summary> + public NativeArray<int> tags; + + /// <summary> + /// Normals of all nodes. + /// If height testing is disabled the normal will be (0,1,0) for all nodes. + /// If a node doesn't exist (only happens in layered grid graphs) or if the height raycast didn't hit anything then the normal will be (0,0,0). + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid + /// - BeforeConnections: Valid + /// - AfterConnections: Valid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// </summary> + public NativeArray<float4> normals; + + /// <summary> + /// Walkability of all nodes before erosion happens. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid (it will be combined with collision testing later) + /// - BeforeConnections: Valid + /// - AfterConnections: Valid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// </summary> + public NativeArray<bool> walkable; + + /// <summary> + /// Walkability of all nodes after erosion happens. This is the final walkability of the nodes. + /// If no erosion is used then the data will just be copied from the <see cref="walkable"/> array. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Invalid + /// - BeforeConnections: Invalid + /// - AfterConnections: Invalid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// </summary> + public NativeArray<bool> walkableWithErosion; + + + /// <summary> + /// True if the data may have multiple layers. + /// For layered data the nodes are laid out as `data[y*width*depth + z*width + x]`. + /// For non-layered data the nodes are laid out as `data[z*width + x]` (which is equivalent to the above layout assuming y=0). + /// + /// This also affects how node connections are stored. You can use <see cref="LayeredGridAdjacencyMapper"/> and <see cref="FlatGridAdjacencyMapper"/> to access + /// connections for the different data layouts. + /// </summary> + public bool layeredDataLayout; + + public void AllocateBuffers (JobDependencyTracker dependencyTracker) { + Profiler.BeginSample("Allocating buffers"); + // Allocate buffers for jobs + // Allocating buffers with uninitialized memory is much faster if no jobs assume anything about their contents + if (dependencyTracker != null) { + positions = dependencyTracker.NewNativeArray<Vector3>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + normals = dependencyTracker.NewNativeArray<float4>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + connections = dependencyTracker.NewNativeArray<ulong>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + penalties = dependencyTracker.NewNativeArray<uint>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkable = dependencyTracker.NewNativeArray<bool>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkableWithErosion = dependencyTracker.NewNativeArray<bool>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + tags = dependencyTracker.NewNativeArray<int>(numNodes, allocationMethod, NativeArrayOptions.ClearMemory); + } else { + positions = new NativeArray<Vector3>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + normals = new NativeArray<float4>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + connections = new NativeArray<ulong>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + penalties = new NativeArray<uint>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkable = new NativeArray<bool>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkableWithErosion = new NativeArray<bool>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + tags = new NativeArray<int>(numNodes, allocationMethod, NativeArrayOptions.ClearMemory); + } + Profiler.EndSample(); + } + + public void TrackBuffers (JobDependencyTracker dependencyTracker) { + if (positions.IsCreated) dependencyTracker.Track(positions); + if (normals.IsCreated) dependencyTracker.Track(normals); + if (connections.IsCreated) dependencyTracker.Track(connections); + if (penalties.IsCreated) dependencyTracker.Track(penalties); + if (walkable.IsCreated) dependencyTracker.Track(walkable); + if (walkableWithErosion.IsCreated) dependencyTracker.Track(walkableWithErosion); + if (tags.IsCreated) dependencyTracker.Track(tags); + } + + public void PersistBuffers (JobDependencyTracker dependencyTracker) { + dependencyTracker.Persist(positions); + dependencyTracker.Persist(normals); + dependencyTracker.Persist(connections); + dependencyTracker.Persist(penalties); + dependencyTracker.Persist(walkable); + dependencyTracker.Persist(walkableWithErosion); + dependencyTracker.Persist(tags); + } + + public void Dispose () { + bounds = default; + numNodes = 0; + if (positions.IsCreated) positions.Dispose(); + if (normals.IsCreated) normals.Dispose(); + if (connections.IsCreated) connections.Dispose(); + if (penalties.IsCreated) penalties.Dispose(); + if (walkable.IsCreated) walkable.Dispose(); + if (walkableWithErosion.IsCreated) walkableWithErosion.Dispose(); + if (tags.IsCreated) tags.Dispose(); + } + + public JobHandle Rotate2D (int dx, int dz, JobHandle dependency) { + var size = bounds.size; + unsafe { + var jobs = stackalloc JobHandle[7]; + jobs[0] = positions.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[1] = normals.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[2] = connections.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[3] = penalties.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[4] = walkable.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[5] = walkableWithErosion.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[6] = tags.Rotate3D(size, dx, dz).Schedule(dependency); + return JobHandleUnsafeUtility.CombineDependencies(jobs, 7); + } + } + + public void ResizeLayerCount (int layerCount, JobDependencyTracker dependencyTracker) { + if (layerCount > layers) { + var oldData = this; + this.bounds.max.y = layerCount; + this.numNodes = bounds.volume; + this.AllocateBuffers(dependencyTracker); + // Ensure the normals for the upper layers are zeroed out. + // All other node data in the upper layers can be left uninitialized. + this.normals.MemSet(float4.zero).Schedule(dependencyTracker); + this.walkable.MemSet(false).Schedule(dependencyTracker); + this.walkableWithErosion.MemSet(false).Schedule(dependencyTracker); + new JobCopyBuffers { + input = oldData, + output = this, + copyPenaltyAndTags = true, + bounds = oldData.bounds, + }.Schedule(dependencyTracker); + } + if (layerCount < layers) { + throw new System.ArgumentException("Cannot reduce the number of layers"); + } + } + + struct LightReader : GridIterationUtilities.ISliceAction { + public GridNodeBase[] nodes; + public UnsafeSpan<Vector3> nodePositions; + public UnsafeSpan<bool> nodeWalkable; + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public void Execute (uint outerIdx, uint innerIdx) { + // The data bounds may have more layers than the existing nodes if a new layer is being added. + // We can only copy from the nodes that exist. + if (outerIdx < nodes.Length) { + var node = nodes[outerIdx]; + if (node != null) { + nodePositions[innerIdx] = (Vector3)node.position; + nodeWalkable[innerIdx] = node.Walkable; + return; + } + } + + // Fallback in case the node was null (only happens for layered grid graphs), + // or if we are adding more layers to the graph, in which case we are outside + // the bounds of the nodes array. + nodePositions[innerIdx] = Vector3.zero; + nodeWalkable[innerIdx] = false; + } + } + + public void ReadFromNodesForConnectionCalculations (GridNodeBase[] nodes, Slice3D slice, JobHandle nodesDependsOn, NativeArray<float4> graphNodeNormals, JobDependencyTracker dependencyTracker) { + bounds = slice.slice; + numNodes = slice.slice.volume; + + Profiler.BeginSample("Allocating buffers"); + positions = new NativeArray<Vector3>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + normals = new NativeArray<float4>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + connections = new NativeArray<ulong>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkableWithErosion = new NativeArray<bool>(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + Profiler.EndSample(); + + Profiler.BeginSample("Reading node data"); + var reader = new LightReader { + nodes = nodes, + nodePositions = this.positions.AsUnsafeSpan(), + nodeWalkable = this.walkableWithErosion.AsUnsafeSpan(), + }; + GridIterationUtilities.ForEachCellIn3DSlice(slice, ref reader); + Profiler.EndSample(); + + ReadNodeNormals(slice, graphNodeNormals, dependencyTracker); + } + + void ReadNodeNormals (Slice3D slice, NativeArray<float4> graphNodeNormals, JobDependencyTracker dependencyTracker) { + UnityEngine.Assertions.Assert.IsTrue(graphNodeNormals.IsCreated); + // Read the normal data from the graphNodeNormals array and copy it to the nodeNormals array. + // The nodeArrayBounds may have fewer layers than the readBounds if layers are being added. + // This means we can copy only a subset of the normals. + // We MemSet the array to zero first to avoid any uninitialized data remaining. + // TODO: Do clamping in caller + //var clampedReadBounds = new IntBounds(readBounds.min, new int3(readBounds.max.x, math.min(nodeArrayBounds.y, readBounds.max.y), readBounds.max.z)); + if (dependencyTracker != null) { + normals.MemSet(float4.zero).Schedule(dependencyTracker); + new JobCopyRectangle<float4> { + input = graphNodeNormals, + output = normals, + inputSlice = slice, + outputSlice = new Slice3D(bounds, slice.slice), + }.Schedule(dependencyTracker); + } else { + Profiler.BeginSample("ReadNodeNormals"); + normals.AsUnsafeSpan().FillZeros(); + JobCopyRectangle<float4>.Copy(graphNodeNormals, normals, slice, new Slice3D(bounds, slice.slice)); + Profiler.EndSample(); + } + } + + public static GridGraphNodeData ReadFromNodes (GridNodeBase[] nodes, Slice3D slice, JobHandle nodesDependsOn, NativeArray<float4> graphNodeNormals, Allocator allocator, bool layeredDataLayout, JobDependencyTracker dependencyTracker) { + var nodeData = new GridGraphNodeData { + allocationMethod = allocator, + numNodes = slice.slice.volume, + bounds = slice.slice, + layeredDataLayout = layeredDataLayout, + }; + nodeData.AllocateBuffers(dependencyTracker); + + // This is a managed type, we need to trick Unity to allow this inside of a job + var nodesHandle = System.Runtime.InteropServices.GCHandle.Alloc(nodes); + + var job = new JobReadNodeData { + nodesHandle = nodesHandle, + nodePositions = nodeData.positions, + nodePenalties = nodeData.penalties, + nodeTags = nodeData.tags, + nodeConnections = nodeData.connections, + nodeWalkableWithErosion = nodeData.walkableWithErosion, + nodeWalkable = nodeData.walkable, + slice = slice, + }.ScheduleBatch(nodeData.numNodes, math.max(2000, nodeData.numNodes/16), dependencyTracker, nodesDependsOn); + + dependencyTracker.DeferFree(nodesHandle, job); + + if (graphNodeNormals.IsCreated) nodeData.ReadNodeNormals(slice, graphNodeNormals, dependencyTracker); + return nodeData; + } + + public GridGraphNodeData ReadFromNodesAndCopy (GridNodeBase[] nodes, Slice3D slice, JobHandle nodesDependsOn, NativeArray<float4> graphNodeNormals, bool copyPenaltyAndTags, JobDependencyTracker dependencyTracker) { + var newData = GridGraphNodeData.ReadFromNodes(nodes, slice, nodesDependsOn, graphNodeNormals, allocationMethod, layeredDataLayout, dependencyTracker); + // Overwrite a rectangle in the center with the data from this object. + // In the end we will have newly calculated data in the middle and data read from nodes along the borders + newData.CopyFrom(this, copyPenaltyAndTags, dependencyTracker); + return newData; + } + + public void CopyFrom(GridGraphNodeData other, bool copyPenaltyAndTags, JobDependencyTracker dependencyTracker) => CopyFrom(other, IntBounds.Intersection(bounds, other.bounds), copyPenaltyAndTags, dependencyTracker); + + public void CopyFrom (GridGraphNodeData other, IntBounds bounds, bool copyPenaltyAndTags, JobDependencyTracker dependencyTracker) { + var job = new JobCopyBuffers { + input = other, + output = this, + copyPenaltyAndTags = copyPenaltyAndTags, + bounds = bounds, + }; + if (dependencyTracker != null) { + job.Schedule(dependencyTracker); + } else { +#if UNITY_2022_2_OR_NEWER + job.RunByRef(); +#else + job.Run(); +#endif + } + } + + public JobHandle AssignToNodes (GridNodeBase[] nodes, int3 nodeArrayBounds, IntBounds writeMask, uint graphIndex, JobHandle nodesDependsOn, JobDependencyTracker dependencyTracker) { + // This is a managed type, we need to trick Unity to allow this inside of a job + var nodesHandle = System.Runtime.InteropServices.GCHandle.Alloc(nodes); + + // Assign the data to the nodes (in parallel for performance) + // This will also dirty all nodes, but that is a thread-safe operation. + var job2 = new JobWriteNodeData { + nodesHandle = nodesHandle, + graphIndex = graphIndex, + nodePositions = positions, + nodePenalties = penalties, + nodeTags = tags, + nodeConnections = connections, + nodeWalkableWithErosion = walkableWithErosion, + nodeWalkable = walkable, + nodeArrayBounds = nodeArrayBounds, + dataBounds = bounds, + writeMask = writeMask, + }.ScheduleBatch(writeMask.volume, math.max(1000, writeMask.volume/16), dependencyTracker, nodesDependsOn); + + dependencyTracker.DeferFree(nodesHandle, job2); + return job2; + } + } + + public struct GridGraphScanData { + /// <summary> + /// Tracks dependencies between jobs to allow parallelism without tediously specifying dependencies manually. + /// Always use when scheduling jobs. + /// </summary> + public JobDependencyTracker dependencyTracker; + + /// <summary>The up direction of the graph, in world space</summary> + public Vector3 up; + + /// <summary>Transforms graph-space to world space</summary> + public GraphTransform transform; + + /// <summary>Data for all nodes in the graph update that is being calculated</summary> + public GridGraphNodeData nodes; + + /// <summary> + /// Bounds of the data arrays. + /// Deprecated: Use nodes.bounds or heightHitsBounds depending on if you are using the heightHits array or not + /// </summary> + [System.Obsolete("Use nodes.bounds or heightHitsBounds depending on if you are using the heightHits array or not")] + public IntBounds bounds => nodes.bounds; + + /// <summary> + /// True if the data may have multiple layers. + /// For layered data the nodes are laid out as `data[y*width*depth + z*width + x]`. + /// For non-layered data the nodes are laid out as `data[z*width + x]` (which is equivalent to the above layout assuming y=0). + /// + /// Deprecated: Use nodes.layeredDataLayout instead + /// </summary> + [System.Obsolete("Use nodes.layeredDataLayout instead")] + public bool layeredDataLayout => nodes.layeredDataLayout; + + /// <summary> + /// Raycasts hits used for height testing. + /// This data is only valid if height testing is enabled, otherwise the array is uninitialized (heightHits.IsCreated will be false). + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid (if height testing is enabled) + /// - BeforeConnections: Valid (if height testing is enabled) + /// - AfterConnections: Valid (if height testing is enabled) + /// - AfterErosion: Valid (if height testing is enabled) + /// - PostProcess: Valid (if height testing is enabled) + /// + /// Warning: This array does not have the same size as the arrays in <see cref="nodes"/>. It will usually be slightly smaller. See <see cref="heightHitsBounds"/>. + /// </summary> + public NativeArray<RaycastHit> heightHits; + + /// <summary> + /// Bounds for the <see cref="heightHits"/> array. + /// + /// During an update, the scan data may contain more nodes than we are doing height testing for. + /// For a few nodes around the update, the data will be read from the existing graph, instead. This is done for performance. + /// This means that there may not be any height testing information these nodes. + /// However, all nodes that will be written to will always have height testing information. + /// </summary> + public IntBounds heightHitsBounds; + + /// <summary> + /// Node positions. + /// Deprecated: Use <see cref="nodes.positions"/> instead + /// </summary> + [System.Obsolete("Use nodes.positions instead")] + public NativeArray<Vector3> nodePositions => nodes.positions; + + /// <summary> + /// Node connections. + /// Deprecated: Use <see cref="nodes.connections"/> instead + /// </summary> + [System.Obsolete("Use nodes.connections instead")] + public NativeArray<ulong> nodeConnections => nodes.connections; + + /// <summary> + /// Node penalties. + /// Deprecated: Use <see cref="nodes.penalties"/> instead + /// </summary> + [System.Obsolete("Use nodes.penalties instead")] + public NativeArray<uint> nodePenalties => nodes.penalties; + + /// <summary> + /// Node tags. + /// Deprecated: Use <see cref="nodes.tags"/> instead + /// </summary> + [System.Obsolete("Use nodes.tags instead")] + public NativeArray<int> nodeTags => nodes.tags; + + /// <summary> + /// Node normals. + /// Deprecated: Use <see cref="nodes.normals"/> instead + /// </summary> + [System.Obsolete("Use nodes.normals instead")] + public NativeArray<float4> nodeNormals => nodes.normals; + + /// <summary> + /// Node walkability. + /// Deprecated: Use <see cref="nodes.walkable"/> instead + /// </summary> + [System.Obsolete("Use nodes.walkable instead")] + public NativeArray<bool> nodeWalkable => nodes.walkable; + + /// <summary> + /// Node walkability with erosion. + /// Deprecated: Use <see cref="nodes.walkableWithErosion"/> instead + /// </summary> + [System.Obsolete("Use nodes.walkableWithErosion instead")] + public NativeArray<bool> nodeWalkableWithErosion => nodes.walkableWithErosion; + + public void SetDefaultPenalties (uint initialPenalty) { + nodes.penalties.MemSet(initialPenalty).Schedule(dependencyTracker); + } + + public void SetDefaultNodePositions (GraphTransform transform) { + new JobNodeGridLayout { + graphToWorld = transform.matrix, + bounds = nodes.bounds, + nodePositions = nodes.positions, + }.Schedule(dependencyTracker); + } + + public JobHandle HeightCheck (GraphCollision collision, int maxHits, IntBounds recalculationBounds, NativeArray<int> outLayerCount, float characterHeight, Allocator allocator) { + // For some reason the physics code crashes when allocating raycastCommands with UninitializedMemory, even though I have verified that every + // element in the array is set to a well defined value before the physics code gets to it... Mysterious. + var cellCount = recalculationBounds.size.x * recalculationBounds.size.z; + var raycastCommands = dependencyTracker.NewNativeArray<RaycastCommand>(cellCount, allocator, NativeArrayOptions.ClearMemory); + + heightHits = dependencyTracker.NewNativeArray<RaycastHit>(cellCount * maxHits, allocator, NativeArrayOptions.ClearMemory); + heightHitsBounds = recalculationBounds; + + // Due to floating point inaccuracies we don't want the rays to end *exactly* at the base of the graph + // The rays may or may not hit colliders with the exact same y coordinate. + // We extend the rays a bit to ensure they always hit + const float RayLengthMargin = 0.01f; + var prepareJob = new JobPrepareGridRaycast { + graphToWorld = transform.matrix, + bounds = recalculationBounds, + physicsScene = Physics.defaultPhysicsScene, + raycastOffset = up * collision.fromHeight, + raycastDirection = -up * (collision.fromHeight + RayLengthMargin), + raycastMask = collision.heightMask, + raycastCommands = raycastCommands, + }.Schedule(dependencyTracker); + + if (maxHits > 1) { + // Skip this distance between each hit. + // It is pretty arbitrarily chosen, but it must be lower than characterHeight. + // If it would be set too low then many thin colliders stacked on top of each other could lead to a very large number of hits + // that will not lead to any walkable nodes anyway. + float minStep = characterHeight * 0.5f; + var dependency = new JobRaycastAll(raycastCommands, heightHits, Physics.defaultPhysicsScene, maxHits, allocator, dependencyTracker, minStep).Schedule(prepareJob); + + dependency = new JobMaxHitCount { + hits = heightHits, + maxHits = maxHits, + layerStride = cellCount, + maxHitCount = outLayerCount, + }.Schedule(dependency); + + return dependency; + } else { + dependencyTracker.ScheduleBatch(raycastCommands, heightHits, 2048); + outLayerCount[0] = 1; + return default; + } + } + + public void CopyHits (IntBounds recalculationBounds) { + // Copy the hit points and normals to separate arrays + // Ensure the normals for the upper layers are zeroed out. + nodes.normals.MemSet(float4.zero).Schedule(dependencyTracker); + new JobCopyHits { + hits = heightHits, + points = nodes.positions, + normals = nodes.normals, + slice = new Slice3D(nodes.bounds, recalculationBounds), + }.Schedule(dependencyTracker); + } + + public void CalculateWalkabilityFromHeightData (bool useRaycastNormal, bool unwalkableWhenNoGround, float maxSlope, float characterHeight) { + new JobNodeWalkability { + useRaycastNormal = useRaycastNormal, + unwalkableWhenNoGround = unwalkableWhenNoGround, + maxSlope = maxSlope, + up = up, + nodeNormals = nodes.normals, + nodeWalkable = nodes.walkable, + nodePositions = nodes.positions.Reinterpret<float3>(), + characterHeight = characterHeight, + layerStride = nodes.bounds.size.x*nodes.bounds.size.z, + }.Schedule(dependencyTracker); + } + + public IEnumerator<JobHandle> CollisionCheck (GraphCollision collision, IntBounds calculationBounds) { + if (collision.type == ColliderType.Ray && !collision.use2D) { + var collisionCheckResult = dependencyTracker.NewNativeArray<bool>(nodes.numNodes, nodes.allocationMethod, NativeArrayOptions.UninitializedMemory); + collision.JobCollisionRay(nodes.positions, collisionCheckResult, up, nodes.allocationMethod, dependencyTracker); + nodes.walkable.BitwiseAndWith(collisionCheckResult).WithLength(nodes.numNodes).Schedule(dependencyTracker); + return null; + +// Before Unity 2023.3, these features compile, but they will cause memory corruption in some cases, due to a bug in Unity +#if UNITY_2022_2_OR_NEWER && UNITY_2023_3_OR_NEWER && UNITY_HAS_FIXED_MEMORY_CORRUPTION_ISSUE + } else if (collision.type == ColliderType.Capsule && !collision.use2D) { + var collisionCheckResult = dependencyTracker.NewNativeArray<bool>(nodes.numNodes, nodes.allocationMethod, NativeArrayOptions.UninitializedMemory); + collision.JobCollisionCapsule(nodes.positions, collisionCheckResult, up, nodes.allocationMethod, dependencyTracker); + nodes.walkable.BitwiseAndWith(collisionCheckResult).WithLength(nodes.numNodes).Schedule(dependencyTracker); + return null; + } else if (collision.type == ColliderType.Sphere && !collision.use2D) { + var collisionCheckResult = dependencyTracker.NewNativeArray<bool>(nodes.numNodes, nodes.allocationMethod, NativeArrayOptions.UninitializedMemory); + collision.JobCollisionSphere(nodes.positions, collisionCheckResult, up, nodes.allocationMethod, dependencyTracker); + nodes.walkable.BitwiseAndWith(collisionCheckResult).WithLength(nodes.numNodes).Schedule(dependencyTracker); + return null; +#endif + } else { + // This part can unfortunately not be jobified yet + return new JobCheckCollisions { + nodePositions = nodes.positions, + collisionResult = nodes.walkable, + collision = collision, + }.ExecuteMainThreadJob(dependencyTracker); + } + } + + public void Connections (float maxStepHeight, bool maxStepUsesSlope, IntBounds calculationBounds, NumNeighbours neighbours, bool cutCorners, bool use2D, bool useErodedWalkability, float characterHeight) { + var job = new JobCalculateGridConnections { + maxStepHeight = maxStepHeight, + maxStepUsesSlope = maxStepUsesSlope, + up = up, + bounds = calculationBounds.Offset(-nodes.bounds.min), + arrayBounds = nodes.bounds.size, + neighbours = neighbours, + use2D = use2D, + cutCorners = cutCorners, + nodeWalkable = (useErodedWalkability ? nodes.walkableWithErosion : nodes.walkable).AsUnsafeSpanNoChecks(), + nodePositions = nodes.positions.AsUnsafeSpanNoChecks(), + nodeNormals = nodes.normals.AsUnsafeSpanNoChecks(), + nodeConnections = nodes.connections.AsUnsafeSpanNoChecks(), + characterHeight = characterHeight, + layeredDataLayout = nodes.layeredDataLayout, + }; + + if (dependencyTracker != null) { + job.ScheduleBatch(calculationBounds.size.z, 20, dependencyTracker); + } else { + job.RunBatch(calculationBounds.size.z); + } + + // For single layer graphs this will have already been done in the JobCalculateGridConnections job + // but for layered grid graphs we need to handle things differently because the data layout is different. + // It needs to be done after all axis aligned connections have been calculated. + if (nodes.layeredDataLayout) { + var job2 = new JobFilterDiagonalConnections { + slice = new Slice3D(nodes.bounds, calculationBounds), + neighbours = neighbours, + cutCorners = cutCorners, + nodeConnections = nodes.connections.AsUnsafeSpanNoChecks(), + }; + if (dependencyTracker != null) { + job2.ScheduleBatch(calculationBounds.size.z, 20, dependencyTracker); + } else { + job2.RunBatch(calculationBounds.size.z); + } + } + } + + public void Erosion (NumNeighbours neighbours, int erodeIterations, IntBounds erosionWriteMask, bool erosionUsesTags, int erosionStartTag, int erosionTagsPrecedenceMask) { + if (!nodes.layeredDataLayout) { + new JobErosion<FlatGridAdjacencyMapper> { + bounds = nodes.bounds, + writeMask = erosionWriteMask, + neighbours = neighbours, + nodeConnections = nodes.connections, + erosion = erodeIterations, + nodeWalkable = nodes.walkable, + outNodeWalkable = nodes.walkableWithErosion, + nodeTags = nodes.tags, + erosionUsesTags = erosionUsesTags, + erosionStartTag = erosionStartTag, + erosionTagsPrecedenceMask = erosionTagsPrecedenceMask, + }.Schedule(dependencyTracker); + } else { + new JobErosion<LayeredGridAdjacencyMapper> { + bounds = nodes.bounds, + writeMask = erosionWriteMask, + neighbours = neighbours, + nodeConnections = nodes.connections, + erosion = erodeIterations, + nodeWalkable = nodes.walkable, + outNodeWalkable = nodes.walkableWithErosion, + nodeTags = nodes.tags, + erosionUsesTags = erosionUsesTags, + erosionStartTag = erosionStartTag, + erosionTagsPrecedenceMask = erosionTagsPrecedenceMask, + }.Schedule(dependencyTracker); + } + } + + public void AssignNodeConnections (GridNodeBase[] nodes, int3 nodeArrayBounds, IntBounds writeBounds) { + var bounds = this.nodes.bounds; + var writeDataOffset = writeBounds.min - bounds.min; + var nodeConnections = this.nodes.connections.AsUnsafeReadOnlySpan(); + for (int y = 0; y < writeBounds.size.y; y++) { + var yoffset = (y + writeBounds.min.y)*nodeArrayBounds.x*nodeArrayBounds.z; + for (int z = 0; z < writeBounds.size.z; z++) { + var zoffset = yoffset + (z + writeBounds.min.z)*nodeArrayBounds.x + writeBounds.min.x; + var zoffset2 = (y+writeDataOffset.y)*bounds.size.x*bounds.size.z + (z+writeDataOffset.z)*bounds.size.x + writeDataOffset.x; + for (int x = 0; x < writeBounds.size.x; x++) { + var node = nodes[zoffset + x]; + var dataIdx = zoffset2 + x; + var conn = nodeConnections[dataIdx]; + + if (node == null) continue; + + if (node is LevelGridNode lgnode) { + lgnode.SetAllConnectionInternal(conn); + } else { + var gnode = node as GridNode; + gnode.SetAllConnectionInternal((int)conn); + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs.meta new file mode 100644 index 0000000..c108af9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cd8d2fda4c637484c806417e77602960 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs new file mode 100644 index 0000000..8d5485b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs @@ -0,0 +1,261 @@ +using Pathfinding.Jobs; +using Unity.Collections; +using Unity.Mathematics; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid { + /// <summary> + /// Helpers for iterating over grid graph data. + /// + /// This is a helper for iterating over grid graph data, which is typically stored in an array of size width*layers*depth (x*y*z). + /// It is used internally by grid graph jobs, and can also be used by custom grid graph rules. + /// + /// See: grid-rules-write (view in online documentation for working links) + /// </summary> + public static class GridIterationUtilities { + /// <summary>Callback struct for <see cref="ForEachCellIn3DSlice"/></summary> + public interface ISliceAction { + void Execute(uint outerIdx, uint innerIdx); + } + + /// <summary>Callback struct for <see cref="ForEachCellIn3DSlice"/></summary> + public interface ISliceActionWithCoords { + void Execute(uint outerIdx, uint innerIdx, int3 innerCoords); + } + + /// <summary>Callback struct for <see cref="ForEachCellIn3DArray"/></summary> + public interface ICellAction { + void Execute(uint idx, int x, int y, int z); + } + + /// <summary> + /// Iterates over a slice of a 3D array. + /// + /// This is a helper for iterating over grid graph data, which is typically stored in an array of size width*layers*depth (x*y*z). + /// + /// In burst-compiled code, this will be essentially as fast as writing the loop code yourself. In C#, it is marginally slower than writing the loop code yourself. + /// + /// [Open online documentation to see images] + /// </summary> + /// <param name="slice">Bounds of the slice and the size of the outer array it is relative to.</param> + /// <param name="action">Your callback struct. The Execute method on the callback struct will be called for each element in the slice. It will be passed both the index in the slice and the index in the outer array.</param> + public static void ForEachCellIn3DSlice<T>(Slice3D slice, ref T action) where T : struct, ISliceAction { + var size = slice.slice.size; + var(strideX, strideY, strideZ) = slice.outerStrides; + var outerOffset = slice.outerStartIndex; + uint i = 0; + for (int y = 0; y < size.y; y++) { + for (int z = 0; z < size.z; z++) { + int offset2 = y*strideY + z*strideZ + outerOffset; + for (int x = 0; x < size.x; x++, i++) { + action.Execute((uint)(offset2 + x), i); + } + } + } + } + + /// <summary> + /// Iterates over a slice of a 3D array. + /// + /// This is a helper for iterating over grid graph data, which is typically stored in an array of size width*layers*depth (x*y*z). + /// + /// In burst-compiled code, this will be essentially as fast as writing the loop code yourself. In C#, it is marginally slower than writing the loop code yourself. + /// + /// [Open online documentation to see images] + /// </summary> + /// <param name="slice">Bounds of the slice and the size of the outer array it is relative to.</param> + /// <param name="action">Your callback struct. The Execute method on the callback struct will be called for each element in the slice. It will be passed both the index in the slice and the index in the outer array.</param> + public static void ForEachCellIn3DSliceWithCoords<T>(Slice3D slice, ref T action) where T : struct, ISliceActionWithCoords { + var size = slice.slice.size; + var(strideX, strideY, strideZ) = slice.outerStrides; + var outerOffset = slice.outerStartIndex; + uint i = (uint)(size.x*size.y*size.z) - 1; + for (int y = size.y - 1; y >= 0; y--) { + for (int z = size.z - 1; z >= 0; z--) { + int offset2 = y*strideY + z*strideZ + outerOffset; + for (int x = size.x - 1; x >= 0; x--, i--) { + action.Execute((uint)(offset2 + x), i, new int3(x, y, z)); + } + } + } + } + + /// <summary> + /// Iterates over a 3D array. + /// + /// This is a helper for iterating over grid graph data, which is typically stored in an array of size width*layers*depth (x*y*z). + /// + /// In burst-compiled code, this will be essentially as fast as writing the loop code yourself. In C#, it is marginally slower than writing the loop code yourself. + /// </summary> + /// <param name="size">Size of the array.</param> + /// <param name="action">Your callback struct. The Execute method on the callback struct will be called for each element in the array. It will be passed the x, y and z coordinates of the element as well as the index in the array.</param> + public static void ForEachCellIn3DArray<T>(int3 size, ref T action) where T : struct, ICellAction { + uint i = (uint)(size.x*size.y*size.z) - 1; + for (int y = size.y - 1; y >= 0; y--) { + for (int z = size.z - 1; z >= 0; z--) { + for (int x = size.x - 1; x >= 0; x--, i--) { + action.Execute(i, x, y, z); + } + } + } + } + + /// <summary> + /// Helper interface for modifying nodes. + /// This is used by the <see cref="GridIterationUtilities.ForEachNode"/> function. + /// </summary> + public interface INodeModifier { + /// <summary> + /// Called for every node that is being updated. + /// + /// See: gridgraphrule-burst (view in online documentation for working links) for example usage. + /// </summary> + /// <param name="dataIndex">Index of the node. This is the index in the data arrays for the graph update, not necessarily the index in the graph.</param> + /// <param name="dataX">X coordinate of the node, relative to the updated region.</param> + /// <param name="dataLayer">Layer (Y) coordinate of the node, relative to the updated region.</param> + /// <param name="dataZ">Z coordinate of the node, relative to the updated region.</param> + void ModifyNode(int dataIndex, int dataX, int dataLayer, int dataZ); + } + + /// <summary> + /// Iterate through all nodes that exist. + /// + /// See: grid-rules-write (view in online documentation for working links) for example usage. + /// </summary> + /// <param name="arrayBounds">Size of the rectangle of the grid graph that is being updated/scanned</param> + /// <param name="nodeNormals">Data for all node normals. This is used to determine if a node exists (important for layered grid graphs).</param> + /// <param name="callback">The ModifyNode method on the callback struct will be called for each node.</param> + public static void ForEachNode<T>(int3 arrayBounds, NativeArray<float4> nodeNormals, ref T callback) where T : struct, INodeModifier { + Assert.IsTrue(nodeNormals.Length == arrayBounds.x * arrayBounds.y * arrayBounds.z); + int i = 0; + + for (int y = 0; y < arrayBounds.y; y++) { + for (int z = 0; z < arrayBounds.z; z++) { + for (int x = 0; x < arrayBounds.x; x++, i++) { + // Check if the node exists at all + // This is important for layered grid graphs + // A normal is never zero otherwise + if (math.any(nodeNormals[i])) { + callback.ModifyNode(i, x, y, z); + } + } + } + } + } + + /// <summary> + /// Helper interface for modifying node connections. + /// This is used by the <see cref="GridIterationUtilities.FilterNodeConnections"/> function. + /// </summary> + public interface IConnectionFilter { + /// <summary> + /// Returns true if the connection should be enabled. + /// + /// See: gridgraphrule-connection-filter (view in online documentation for working links) for example usage. + /// See: <see cref="GridIterationUtilities.GetNeighbourDataIndex"/> + /// </summary> + /// <param name="dataIndex">Index of the node for which the connection is being tested. This is the index in the data arrays for the graph update, not necessarily the index in the graph.</param> + /// <param name="dataX">X coordinate of the node for which the connection is being tested, relative to the updated region.</param> + /// <param name="dataLayer">Layer (Y) coordinate of the node for which the connection is being tested, relative to the updated region.</param> + /// <param name="dataZ">Z coordinate of the node for which the connection is being tested, relative to the updated region.</param> + /// <param name="direction">Direction to the neighbour. See \reflink{GridNode.HasConnectionInDirection}.</param> + /// <param name="neighbourDataIndex">Index of the neighbour node. This is the index in the data arrays for the graph update, not necessarily the index in the graph.</param> + bool IsValidConnection(int dataIndex, int dataX, int dataLayer, int dataZ, int direction, int neighbourDataIndex); + } + + /// <summary> + /// Iterate through all enabled connections of all nodes. + /// + /// See: grid-rules-write (view in online documentation for working links) for example usage. + /// </summary> + /// <param name="bounds">Sub-rectangle of the grid graph that is being updated/scanned</param> + /// <param name="nodeConnections">Data with all node connections.</param> + /// <param name="layeredDataLayout">Should be true for layered grid graphs and false otherwise.</param> + /// <param name="filter">Your callback struct. The IsValidConnection method on the callback struct will be called for each connection. If false is returned, the connection will be disabled.</param> + public static void FilterNodeConnections<T>(IntBounds bounds, NativeArray<ulong> nodeConnections, bool layeredDataLayout, ref T filter) where T : struct, IConnectionFilter { + var size = bounds.size; + Assert.IsTrue(nodeConnections.Length == size.x * size.y * size.z); + unsafe { + var neighbourOffsets = stackalloc int[8]; + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * size.x + GridGraph.neighbourXOffsets[i]; + var layerStride = size.x * size.z; + + int nodeIndex = 0; + for (int y = 0; y < size.y; y++) { + for (int z = 0; z < size.z; z++) { + for (int x = 0; x < size.x; x++, nodeIndex++) { + var conn = nodeConnections[nodeIndex]; + if (layeredDataLayout) { + // Layered grid graph + for (int dir = 0; dir < 8; dir++) { + var connectionValue = (int)((conn >> LevelGridNode.ConnectionStride*dir) & LevelGridNode.ConnectionMask); + if (connectionValue != LevelGridNode.NoConnection && !filter.IsValidConnection(nodeIndex, x, y, z, dir, nodeIndex + neighbourOffsets[dir] + (connectionValue - y)*layerStride)) { + conn |= (ulong)LevelGridNode.NoConnection << LevelGridNode.ConnectionStride*dir; + } + } + } else { + // Normal grid graph + // Iterate through all connections on the node + for (int dir = 0; dir < 8; dir++) { + if (((int)conn & (1 << dir)) != 0 && !filter.IsValidConnection(nodeIndex, x, y, z, dir, nodeIndex + neighbourOffsets[dir])) { + conn &= ~(1UL << dir); + } + } + } + nodeConnections[nodeIndex] = conn; + } + } + } + } + } + + /// <summary> + /// Returns the data index for a node's neighbour in the given direction. + /// + /// The bounds, nodeConnections and layeredDataLayout fields can be retrieved from the <see cref="GridGraphRules.Context"/>.data object. + /// + /// Returns: Null if the node has no connection in that direction. Otherwise the data index for that node is returned. + /// + /// See: gridgraphrule-connection-filter (view in online documentation for working links) for example usage. + /// </summary> + /// <param name="bounds">Sub-rectangle of the grid graph that is being updated/scanned</param> + /// <param name="nodeConnections">Data for all node connections</param> + /// <param name="layeredDataLayout">True if this is a layered grid graph</param> + /// <param name="dataX">X coordinate in the data arrays for the node for which you want to get a neighbour</param> + /// <param name="dataLayer">Layer (Y) coordinate in the data arrays for the node for which you want to get a neighbour</param> + /// <param name="dataZ">Z coordinate in the data arrays for the node for which you want to get a neighbour</param> + /// <param name="direction">Direction to the neighbour. See \reflink{GridNode.HasConnectionInDirection}.</param> + public static int? GetNeighbourDataIndex (IntBounds bounds, NativeArray<ulong> nodeConnections, bool layeredDataLayout, int dataX, int dataLayer, int dataZ, int direction) { + // Find the coordinates of the adjacent node + var dx = GridGraph.neighbourXOffsets[direction]; + var dz = GridGraph.neighbourZOffsets[direction]; + + int nx = dataX + dx; + int nz = dataZ + dz; + + // The data arrays are laid out row by row + const int xstride = 1; + var zstride = bounds.size.x; + var ystride = bounds.size.x * bounds.size.z; + + var dataIndex = dataLayer * ystride + dataZ * zstride + dataX * xstride; + var neighbourDataIndex = nz * zstride + nx * xstride; + + if (layeredDataLayout) { + // In a layered grid graph we need to account for nodes in different layers + var ny = (nodeConnections[dataIndex] >> LevelGridNode.ConnectionStride*direction) & LevelGridNode.ConnectionMask; + if (ny == LevelGridNode.NoConnection) return null; + + // For valid nodeConnections arrays this is not necessary as out of bounds connections are not valid and it will thus be caught above in the 'has connection' check. + // But let's be safe in case users do something weird + if (nx < 0 || nz < 0 || nx >= bounds.size.x || nz >= bounds.size.z) throw new System.Exception("Node has an invalid connection to a node outside the bounds of the graph"); + + neighbourDataIndex += (int)ny * ystride; + } else + if ((nodeConnections[dataIndex] & (1UL << direction)) == 0) return null; + + if (nx < 0 || nz < 0 || nx >= bounds.size.x || nz >= bounds.size.z) throw new System.Exception("Node has an invalid connection to a node outside the bounds of the graph"); + return neighbourDataIndex; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs.meta new file mode 100644 index 0000000..ba19ff9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0a7d565c3874ce349a83c260182a8b63 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs.meta new file mode 100644 index 0000000..6cc1028 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: ed5b7a8f175d0794db354a0757bb79ec +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs new file mode 100644 index 0000000..d301d44 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs @@ -0,0 +1,57 @@ +using Pathfinding.Util; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Allocates and deallocates nodes in a grid graph. + /// + /// This will inspect every cell in the dataBounds and allocate or deallocate the node depending on if that slot should have a node or not according to the nodeNormals array (pure zeroes means no node). + /// + /// This is only used for incremental updates of grid graphs. + /// The initial layer of the grid graph (which is always filled with nodes) is allocated in the <see cref="GridGraph.AllocateNodesJob"/> method. + /// </summary> + public struct JobAllocateNodes : IJob { + public AstarPath active; + [ReadOnly] + public NativeArray<float4> nodeNormals; + public IntBounds dataBounds; + public int3 nodeArrayBounds; + public GridNodeBase[] nodes; + public System.Func<GridNodeBase> newGridNodeDelegate; + + public void Execute () { + var size = dataBounds.size; + + // Start at y=1 because all nodes at y=0 are guaranteed to already be allocated (they are always allocated in a layered grid graph). + var nodeNormalsSpan = nodeNormals.AsUnsafeReadOnlySpan(); + for (int y = 1; y < size.y; y++) { + for (int z = 0; z < size.z; z++) { + var rowOffset = ((y + dataBounds.min.y) * nodeArrayBounds.z + (z + dataBounds.min.z)) * nodeArrayBounds.x + dataBounds.min.x; + for (int x = 0; x < size.x; x++) { + var nodeIndex = rowOffset + x; + var shouldHaveNode = math.any(nodeNormalsSpan[nodeIndex]); + var node = nodes[nodeIndex]; + var hasNode = node != null; + if (shouldHaveNode != hasNode) { + if (shouldHaveNode) { + node = nodes[nodeIndex] = newGridNodeDelegate(); + active.InitializeNode(node); + } else { + // Clear custom connections first and clear connections from other nodes to this one + node.ClearCustomConnections(true); + // Clear grid connections without clearing the connections from other nodes to this one (a bit slow) + // Since this is inside a graph update we guarantee that the grid connections will be correct at the end + // of the update anyway + node.ResetConnectionsInternal(); + node.Destroy(); + nodes[nodeIndex] = null; + } + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs.meta new file mode 100644 index 0000000..a2391b4 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 773a80d74b04a904faae186478d396c4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs new file mode 100644 index 0000000..0a14596 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs @@ -0,0 +1,219 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Jobs; +using Pathfinding.Util; +using System.Data; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Calculates the grid connections for all nodes. + /// + /// This is a IJobParallelForBatch job. Calculating the connections in multiple threads is faster, + /// but due to hyperthreading (used on most intel processors) the individual threads will become slower. + /// It is still worth it though. + /// </summary> + [BurstCompile(FloatMode = FloatMode.Fast, CompileSynchronously = true)] + public struct JobCalculateGridConnections : IJobParallelForBatched { + public float maxStepHeight; + /// <summary>Normalized up direction</summary> + public Vector3 up; + public IntBounds bounds; + public int3 arrayBounds; + public NumNeighbours neighbours; + public bool use2D; + public bool cutCorners; + public bool maxStepUsesSlope; + public float characterHeight; + public bool layeredDataLayout; + + [ReadOnly] + public UnsafeSpan<bool> nodeWalkable; + + [ReadOnly] + public UnsafeSpan<float4> nodeNormals; + + [ReadOnly] + public UnsafeSpan<Vector3> nodePositions; + + /// <summary>All bitpacked node connections</summary> + [WriteOnly] + public UnsafeSpan<ulong> nodeConnections; + + public bool allowBoundsChecks => false; + + + /// <summary> + /// Check if a connection to node B is valid. + /// Node A is assumed to be walkable already + /// </summary> + public static bool IsValidConnection (float4 nodePosA, float4 nodeNormalA, bool nodeWalkableB, float4 nodePosB, float4 nodeNormalB, bool maxStepUsesSlope, float maxStepHeight, float4 up) { + if (!nodeWalkableB) return false; + + if (!maxStepUsesSlope) { + // Check their differences along the Y coordinate (well, the up direction really. It is not necessarily the Y axis). + return math.abs(math.dot(up, nodePosB - nodePosA)) <= maxStepHeight; + } else { + float4 v = nodePosB - nodePosA; + float heightDifference = math.dot(v, up); + + // Check if the step is small enough. + // This is a fast path for the common case. + if (math.abs(heightDifference) <= maxStepHeight) return true; + + float4 v_flat = (v - heightDifference * up) * 0.5f; + + // Math! + // Calculates the approximate offset along the up direction + // that the ground will have moved at the midpoint between the + // nodes compared to the nodes' center points. + float NDotU = math.dot(nodeNormalA, up); + float offsetA = -math.dot(nodeNormalA - NDotU * up, v_flat); + + NDotU = math.dot(nodeNormalB, up); + float offsetB = math.dot(nodeNormalB - NDotU * up, v_flat); + + // Check the height difference with slopes taken into account. + // Note that since we also do the heightDifference check above we will ensure slope offsets do not increase the height difference. + // If we allowed this then some connections might not be valid near the start of steep slopes. + return math.abs(heightDifference + offsetB - offsetA) <= maxStepHeight; + } + } + + public void Execute (int start, int count) { + if (layeredDataLayout) ExecuteLayered(start, count); + else ExecuteFlat(start, count); + } + + public void ExecuteFlat (int start, int count) { + if (maxStepHeight <= 0 || use2D) maxStepHeight = float.PositiveInfinity; + + float4 up = new float4(this.up.x, this.up.y, this.up.z, 0); + + NativeArray<int> neighbourOffsets = new NativeArray<int>(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * arrayBounds.x + GridGraph.neighbourXOffsets[i]; + var nodePositions = this.nodePositions.Reinterpret<float3>(); + + // The loop is parallelized over z coordinates + start += bounds.min.z; + for (int z = start; z < start + count; z++) { + var initialConnections = 0xFF; + + // Disable connections to out-of-bounds nodes + // See GridNode.HasConnectionInDirection + if (z == 0) initialConnections &= ~((1 << 0) | (1 << 7) | (1 << 4)); + if (z == arrayBounds.z - 1) initialConnections &= ~((1 << 2) | (1 << 5) | (1 << 6)); + + for (int x = bounds.min.x; x < bounds.max.x; x++) { + int nodeIndex = z * arrayBounds.x + x; + if (!nodeWalkable[nodeIndex]) { + nodeConnections[nodeIndex] = 0; + continue; + } + + // Bitpacked connections + // bit 0 is set if connection 0 is enabled + // bit 1 is set if connection 1 is enabled etc. + int conns = initialConnections; + + // Disable connections to out-of-bounds nodes + if (x == 0) conns &= ~((1 << 3) | (1 << 6) | (1 << 7)); + if (x == arrayBounds.x - 1) conns &= ~((1 << 1) | (1 << 4) | (1 << 5)); + + float4 pos = new float4(nodePositions[nodeIndex], 0); + float4 normal = nodeNormals[nodeIndex]; + + for (int i = 0; i < 8; i++) { + int neighbourIndex = nodeIndex + neighbourOffsets[i]; + if ((conns & (1 << i)) != 0 && !IsValidConnection(pos, normal, nodeWalkable[neighbourIndex], new float4(nodePositions[neighbourIndex], 0), nodeNormals[neighbourIndex], maxStepUsesSlope, maxStepHeight, up)) { + // Enable connection i + conns &= ~(1 << i); + } + } + + nodeConnections[nodeIndex] = (ulong)GridNode.FilterDiagonalConnections(conns, neighbours, cutCorners); + } + } + } + + public void ExecuteLayered (int start, int count) { + if (maxStepHeight <= 0 || use2D) maxStepHeight = float.PositiveInfinity; + + float4 up = new float4(this.up.x, this.up.y, this.up.z, 0); + + NativeArray<int> neighbourOffsets = new NativeArray<int>(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * arrayBounds.x + GridGraph.neighbourXOffsets[i]; + + var layerStride = arrayBounds.z*arrayBounds.x; + start += bounds.min.z; + for (int y = bounds.min.y; y < bounds.max.y; y++) { + // The loop is parallelized over z coordinates + for (int z = start; z < start + count; z++) { + for (int x = bounds.min.x; x < bounds.max.x; x++) { + // Bitpacked connections + ulong conns = 0; + int nodeIndexXZ = z * arrayBounds.x + x; + int nodeIndex = nodeIndexXZ + y * layerStride; + float4 pos = new float4(nodePositions[nodeIndex], 0); + float4 normal = nodeNormals[nodeIndex]; + + if (nodeWalkable[nodeIndex]) { + var ourY = math.dot(up, pos); + + float ourHeight; + if (y == arrayBounds.y-1 || !math.any(nodeNormals[nodeIndex + layerStride])) { + ourHeight = float.PositiveInfinity; + } else { + var nodeAboveNeighbourPos = new float4(nodePositions[nodeIndex + layerStride], 0); + ourHeight = math.max(0, math.dot(up, nodeAboveNeighbourPos) - ourY); + } + + for (int i = 0; i < 8; i++) { + int nx = x + GridGraph.neighbourXOffsets[i]; + int nz = z + GridGraph.neighbourZOffsets[i]; + + // Check if the new position is inside the grid + int conn = LevelGridNode.NoConnection; + if (nx >= 0 && nz >= 0 && nx < arrayBounds.x && nz < arrayBounds.z) { + int neighbourStartIndex = nodeIndexXZ + neighbourOffsets[i]; + for (int y2 = 0; y2 < arrayBounds.y; y2++) { + var neighbourIndex = neighbourStartIndex + y2 * layerStride; + float4 nodePosB = new float4(nodePositions[neighbourIndex], 0); + var neighbourY = math.dot(up, nodePosB); + // Is there a node above this one + float neighbourHeight; + if (y2 == arrayBounds.y-1 || !math.any(nodeNormals[neighbourIndex + layerStride])) { + neighbourHeight = float.PositiveInfinity; + } else { + var nodeAboveNeighbourPos = new float4(nodePositions[neighbourIndex + layerStride], 0); + neighbourHeight = math.max(0, math.dot(up, nodeAboveNeighbourPos) - neighbourY); + } + + float bottom = math.max(neighbourY, ourY); + float top = math.min(neighbourY + neighbourHeight, ourY + ourHeight); + + float dist = top-bottom; + + if (dist >= characterHeight && IsValidConnection(pos, normal, nodeWalkable[neighbourIndex], new float4(nodePositions[neighbourIndex], 0), nodeNormals[neighbourIndex], maxStepUsesSlope, maxStepHeight, up)) { + conn = y2; + } + } + } + + conns |= (ulong)conn << LevelGridNode.ConnectionStride*i; + } + } else { + conns = LevelGridNode.AllConnectionsMask; + } + + nodeConnections[nodeIndex] = conns; + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs.meta new file mode 100644 index 0000000..089d203 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 63dd791a75e95424ea05940b4c155c25 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs new file mode 100644 index 0000000..71a3d2c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs @@ -0,0 +1,33 @@ +using UnityEngine; +using Unity.Collections; +using Pathfinding.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Checks if nodes are obstructed by obstacles or not. + /// + /// See: <see cref="GraphCollision"/> + /// </summary> + struct JobCheckCollisions : IJobTimeSliced { + [ReadOnly] + public NativeArray<Vector3> nodePositions; + public NativeArray<bool> collisionResult; + public GraphCollision collision; + int startIndex; + + public void Execute () { + Execute(TimeSlice.Infinite); + } + + public bool Execute (TimeSlice timeSlice) { + for (int i = startIndex; i < nodePositions.Length; i++) { + collisionResult[i] = collisionResult[i] && collision.Check(nodePositions[i]); + if ((i & 127) == 0 && timeSlice.expired) { + startIndex = i + 1; + return false; + } + } + return true; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs.meta new file mode 100644 index 0000000..9311878 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6977ffeeb891185449eae9f827667558 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs new file mode 100644 index 0000000..84ae59e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs @@ -0,0 +1,28 @@ +#if UNITY_2022_2_OR_NEWER +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Fills the output with true or false depending on if the collider hit was a hit. + /// + /// result[i] = false if hits[i] is a valid hit, otherwise true. + /// </summary> + [BurstCompile] + public struct JobColliderHitsToBooleans : IJob { + [ReadOnly] + public NativeArray<ColliderHit> hits; + + [WriteOnly] + public NativeArray<bool> result; + + public void Execute () { + for (int i = 0; i < hits.Length; i++) { + result[i] = hits[i].instanceID == 0; + } + } + } +} +#endif diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs.meta new file mode 100644 index 0000000..39e8bb7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0e741e2fc3248564998e707ba25fe69c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs new file mode 100644 index 0000000..690448f --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs @@ -0,0 +1,50 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Jobs; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Copies 3D arrays with grid data from one shape to another. + /// + /// Only the data for the nodes that exist in both buffers will be copied. + /// + /// This essentially is several <see cref="JobCopyRectangle"/> jobs in one (to avoid scheduling overhead). + /// See that job for more documentation. + /// </summary> + [BurstCompile] + public struct JobCopyBuffers : IJob { + [ReadOnly] + [DisableUninitializedReadCheck] + public GridGraphNodeData input; + + [WriteOnly] + public GridGraphNodeData output; + public IntBounds bounds; + + public bool copyPenaltyAndTags; + + public void Execute () { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (!input.bounds.Contains(bounds)) throw new System.ArgumentException("Bounds are outside the source buffer"); + if (!output.bounds.Contains(bounds)) throw new System.ArgumentException("Bounds are outside the destination buffer"); +#endif + var inputSlice = new Slice3D(input.bounds, bounds); + var outputSlice = new Slice3D(output.bounds, bounds); + // Note: Having a single job that copies all of the buffers avoids a lot of scheduling overhead. + // We do miss out on parallelization, however for this job it is not that significant. + JobCopyRectangle<Vector3>.Copy(input.positions, output.positions, inputSlice, outputSlice); + JobCopyRectangle<float4>.Copy(input.normals, output.normals, inputSlice, outputSlice); + JobCopyRectangle<ulong>.Copy(input.connections, output.connections, inputSlice, outputSlice); + if (copyPenaltyAndTags) { + JobCopyRectangle<uint>.Copy(input.penalties, output.penalties, inputSlice, outputSlice); + JobCopyRectangle<int>.Copy(input.tags, output.tags, inputSlice, outputSlice); + } + JobCopyRectangle<bool>.Copy(input.walkable, output.walkable, inputSlice, outputSlice); + JobCopyRectangle<bool>.Copy(input.walkableWithErosion, output.walkableWithErosion, inputSlice, outputSlice); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs.meta new file mode 100644 index 0000000..9c60956 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a3c4dcf30e4497e44a9c7421803ec902 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs new file mode 100644 index 0000000..5b5ba35 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs @@ -0,0 +1,190 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Jobs; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Calculates erosion. + /// Note that to ensure that connections are completely up to date after updating a node you + /// have to calculate the connections for both the changed node and its neighbours. + /// + /// In a layered grid graph, this will recalculate the connections for all nodes + /// in the (x,z) cell (it may have multiple layers of nodes). + /// + /// See: CalculateConnections(GridNodeBase) + /// </summary> + [BurstCompile] + public struct JobErosion<AdjacencyMapper> : IJob where AdjacencyMapper : GridAdjacencyMapper, new() { + public IntBounds bounds; + public IntBounds writeMask; + public NumNeighbours neighbours; + public int erosion; + public bool erosionUsesTags; + public int erosionStartTag; + + [ReadOnly] + public NativeArray<ulong> nodeConnections; + + [ReadOnly] + public NativeArray<bool> nodeWalkable; + + [WriteOnly] + public NativeArray<bool> outNodeWalkable; + + public NativeArray<int> nodeTags; + public int erosionTagsPrecedenceMask; + + // Note: the first 3 connections are to nodes with a higher x or z coordinate + // The last 3 connections are to nodes with a lower x or z coordinate + // This is required for the grassfire transform to work properly + // This is a permutation of GridGraph.hexagonNeighbourIndices + static readonly int[] hexagonNeighbourIndices = { 1, 2, 5, 0, 3, 7 }; + + public void Execute () { + var slice = new Slice3D(bounds, bounds); + var size = slice.slice.size; + slice.AssertMatchesOuter(nodeConnections); + slice.AssertMatchesOuter(nodeWalkable); + slice.AssertMatchesOuter(outNodeWalkable); + slice.AssertMatchesOuter(nodeTags); + Assert.IsTrue(bounds.Contains(writeMask)); + + var(outerStrideX, outerStrideY, outerStrideZ) = slice.outerStrides; + var(innerStrideX, innerStrideY, innerStrideZ) = slice.innerStrides; + NativeArray<int> neighbourOffsets = new NativeArray<int>(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * innerStrideZ + GridGraph.neighbourXOffsets[i] * innerStrideX; + + var erosionDistances = new NativeArray<int>(slice.length, Allocator.Temp, NativeArrayOptions.ClearMemory); + var adjacencyMapper = new AdjacencyMapper(); + var layers = adjacencyMapper.LayerCount(slice.slice); + var outerOffset = slice.outerStartIndex; + if (neighbours == NumNeighbours.Six) { + // Use the grassfire transform: https://en.wikipedia.org/wiki/Grassfire_transform extended to hexagonal graphs + for (int z = 1; z < size.z - 1; z++) { + for (int x = 1; x < size.x - 1; x++) { + for (int y = 0; y < layers; y++) { + // Note: This is significantly faster than using csum, because burst can optimize it better + int outerIndex = z * outerStrideZ + x * outerStrideX + y * outerStrideY + outerOffset; + var innerIndexXZ = z * innerStrideZ + x * innerStrideX; + int innerIndex = innerIndexXZ + y * innerStrideY; + int v = int.MaxValue; + for (int i = 3; i < 6; i++) { + int connection = hexagonNeighbourIndices[i]; + if (!adjacencyMapper.HasConnection(outerIndex, connection, nodeConnections)) v = -1; + else v = math.min(v, erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, connection, nodeConnections, neighbourOffsets, innerStrideY)]); + } + + erosionDistances[innerIndex] = v + 1; + } + } + } + + for (int z = size.z - 2; z > 0; z--) { + for (int x = size.x - 2; x > 0; x--) { + for (int y = 0; y < layers; y++) { + int outerIndex = z * outerStrideZ + x * outerStrideX + y * outerStrideY + outerOffset; + var innerIndexXZ = z * innerStrideZ + x * innerStrideX; + int innerIndex = innerIndexXZ + y * innerStrideY; + int v = int.MaxValue; + for (int i = 3; i < 6; i++) { + int connection = hexagonNeighbourIndices[i]; + if (!adjacencyMapper.HasConnection(outerIndex, connection, nodeConnections)) v = -1; + else v = math.min(v, erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, connection, nodeConnections, neighbourOffsets, innerStrideY)]); + } + + erosionDistances[innerIndex] = math.min(erosionDistances[innerIndex], v + 1); + } + } + } + } else { + /* Index offset to get neighbour nodes. Added to a node's index to get a neighbour node index. + * + * \code + * Z + * | + * | + * + * 6 2 5 + * \ | / + * -- 3 - X - 1 ----- X + * / | \ + * 7 0 4 + * + * | + * | + * \endcode + */ + const int DirectionDown = 0; + const int DirectionRight = 1; + const int DirectionUp = 2; + const int DirectionLeft = 3; + + // Use the grassfire transform: https://en.wikipedia.org/wiki/Grassfire_transform + for (int z = 1; z < size.z - 1; z++) { + for (int x = 1; x < size.x - 1; x++) { + for (int y = 0; y < layers; y++) { + int outerIndex = z * outerStrideZ + x * outerStrideX + y * outerStrideY + outerOffset; + var innerIndexXZ = z * innerStrideZ + x * innerStrideX; + int innerIndex = innerIndexXZ + y * innerStrideY; + var v1 = -1; + if (adjacencyMapper.HasConnection(outerIndex, DirectionDown, nodeConnections)) v1 = erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, DirectionDown, nodeConnections, neighbourOffsets, innerStrideY)]; + var v2 = -1; + if (adjacencyMapper.HasConnection(outerIndex, DirectionLeft, nodeConnections)) v2 = erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, DirectionLeft, nodeConnections, neighbourOffsets, innerStrideY)]; + + erosionDistances[innerIndex] = math.min(v1, v2) + 1; + } + } + } + + for (int z = size.z - 2; z > 0; z--) { + for (int x = size.x - 2; x > 0; x--) { + for (int y = 0; y < layers; y++) { + int outerIndex = z * outerStrideZ + x * outerStrideX + y * outerStrideY + outerOffset; + var innerIndexXZ = z * innerStrideZ + x * innerStrideX; + int innerIndex = innerIndexXZ + y * innerStrideY; + var v1 = -1; + if (adjacencyMapper.HasConnection(outerIndex, DirectionUp, nodeConnections)) v1 = erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, DirectionUp, nodeConnections, neighbourOffsets, innerStrideY)]; + var v2 = -1; + if (adjacencyMapper.HasConnection(outerIndex, DirectionRight, nodeConnections)) v2 = erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, DirectionRight, nodeConnections, neighbourOffsets, innerStrideY)]; + + erosionDistances[innerIndex] = math.min(erosionDistances[outerIndex], math.min(v1, v2) + 1); + } + } + } + } + + var relativeWriteMask = writeMask.Offset(-bounds.min); + + // Erosion tags are allowed to overwrite the ones the user specifies, as well as the ones that are already reserved for erosion. + for (int i = erosionStartTag; i < erosionStartTag + erosion; i++) erosionTagsPrecedenceMask |= 1 << i; + + for (int y = relativeWriteMask.min.y; y < relativeWriteMask.max.y; y++) { + for (int z = relativeWriteMask.min.z; z < relativeWriteMask.max.z; z++) { + for (int x = relativeWriteMask.min.x; x < relativeWriteMask.max.x; x++) { + int outerIndex = x * outerStrideX + y * outerStrideY + z * outerStrideZ + outerOffset; + int innerIndex = x * innerStrideX + y * innerStrideY + z * innerStrideZ; + if (erosionUsesTags) { + var prevTag = nodeTags[outerIndex]; + outNodeWalkable[outerIndex] = nodeWalkable[outerIndex]; + + if (erosionDistances[innerIndex] < erosion) { + if (((erosionTagsPrecedenceMask >> prevTag) & 0x1) != 0) { + nodeTags[outerIndex] = nodeWalkable[outerIndex] ? math.min(GraphNode.MaxTagIndex, erosionDistances[innerIndex] + erosionStartTag) : 0; + } + } else if (prevTag >= erosionStartTag && prevTag < erosionStartTag + erosion) { + // If the node already had a tag that was reserved for erosion, but it shouldn't have that tag, then we remove it. + nodeTags[outerIndex] = 0; + } + } else { + outNodeWalkable[outerIndex] = nodeWalkable[outerIndex] & (erosionDistances[innerIndex] >= erosion); + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs.meta new file mode 100644 index 0000000..09484b2 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 12ba2e44c6c183644a4ad6186f4ab21e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs new file mode 100644 index 0000000..6b40a5f --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs @@ -0,0 +1,128 @@ +using Unity.Burst; +using Unity.Collections; +using Unity.Mathematics; +using Pathfinding.Jobs; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Filters out diagonal connections that are not allowed in layered grid graphs. + /// + /// This is a IJobParallelForBatched job which is parallelelized over the z coordinate of the <see cref="slice"/>. + /// + /// The <see cref="JobCalculateGridConnections"/> job will run first, and calculate the connections for all nodes. + /// However, for layered grid graphs, the connections for diagonal nodes may be incorrect, and this + /// post-processing pass is needed to validate the diagonal connections. + /// </summary> + [BurstCompile] + public struct JobFilterDiagonalConnections : IJobParallelForBatched { + public Slice3D slice; + public NumNeighbours neighbours; + public bool cutCorners; + + /// <summary>All bitpacked node connections</summary> + public UnsafeSpan<ulong> nodeConnections; + + public bool allowBoundsChecks => false; + + public void Execute (int start, int count) { + slice.AssertMatchesOuter(nodeConnections); + + // For single layer graphs this will have already been done in the JobCalculateGridConnections job + // but for layered grid graphs we need to handle things differently because the data layout is different + + int3 size = slice.outerSize; + NativeArray<int> neighbourOffsets = new NativeArray<int>(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * size.x + GridGraph.neighbourXOffsets[i]; + + ulong hexagonConnectionMask = 0; + for (int i = 0; i < GridGraph.hexagonNeighbourIndices.Length; i++) hexagonConnectionMask |= (ulong)LevelGridNode.ConnectionMask << (LevelGridNode.ConnectionStride*GridGraph.hexagonNeighbourIndices[i]); + + int adjacencyThreshold = cutCorners ? 1 : 2; + int layerStride = size.x * size.z; + start += slice.slice.min.z; + for (int y = slice.slice.min.y; y < slice.slice.max.y; y++) { + // The loop is parallelized over z coordinates + for (int z = start; z < start + count; z++) { + for (int x = slice.slice.min.x; x < slice.slice.max.x; x++) { + int nodeIndexXZ = z * size.x + x; + int nodeIndex = nodeIndexXZ + y * layerStride; + + switch (neighbours) { + case NumNeighbours.Four: + // Mask out all the diagonal connections + nodeConnections[nodeIndex] = nodeConnections[nodeIndex] | LevelGridNode.DiagonalConnectionsMask; + break; + case NumNeighbours.Eight: + var conns = nodeConnections[nodeIndex]; + + // Skip node if no connections are enabled already + if (conns == LevelGridNode.AllConnectionsMask) continue; + + // When cutCorners is enabled then the diagonal connection is allowed + // if at least one axis aligned connection is adjacent to this diagonal. + // Otherwise both axis aligned connections must be present. + // + // X ----- axis2 + // | \ + // | \ + // | \ + // axis1 diagonal + // + // Z + // | + // | + // + // 6 2 5 + // \ | / + // -- 3 - X - 1 ----- X + // / | \ + // 7 0 4 + // + // | + // | + // + for (int dir = 0; dir < 4; dir++) { + int adjacent = 0; + var axis1 = (conns >> dir*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask; + var axis2 = (conns >> ((dir+1) % 4)*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask; + var diagonal = (conns >> (dir+4)*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask; + + // Check if the diagonal connection is present at all. + // The JobCalculateGridConnections calculated this. + if (diagonal == LevelGridNode.NoConnection) continue; + + if (axis1 != LevelGridNode.NoConnection) { + // We also check that the neighbour node is also connected to the diagonal node + var neighbourDir = (dir + 1) % 4; + var neighbourIndex = nodeIndexXZ + neighbourOffsets[dir] + (int)axis1 * layerStride; + if (((nodeConnections[neighbourIndex] >> neighbourDir*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask) == diagonal) { + adjacent++; + } + } + if (axis2 != LevelGridNode.NoConnection) { + var neighbourDir = dir; + var neighbourIndex = nodeIndexXZ + neighbourOffsets[(dir+1)%4] + (int)axis2 * layerStride; + if (((nodeConnections[neighbourIndex] >> neighbourDir*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask) == diagonal) { + adjacent++; + } + } + + if (adjacent < adjacencyThreshold) conns |= (ulong)LevelGridNode.NoConnection << (dir + 4)*LevelGridNode.ConnectionStride; + } + nodeConnections[nodeIndex] = conns; + break; + case NumNeighbours.Six: + // Hexagon layout + // Note that for layered nodes NoConnection is all bits set (see LevelGridNode.NoConnection) + // So in contrast to the non-layered grid graph we do a bitwise OR here + nodeConnections[nodeIndex] = (nodeConnections[nodeIndex] | ~hexagonConnectionMask) & LevelGridNode.AllConnectionsMask; + break; + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs.meta new file mode 100644 index 0000000..a3d3d9b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e737565f0f6416f4ab1470d4f194ed25 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs new file mode 100644 index 0000000..61707dd --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs @@ -0,0 +1,31 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Calculates if either of the two input hits actually hit something. + /// + /// result[i] = true if hit1[i] or hit2[i] is a valid hit. + /// + /// A valid hit will always have a non-zero normal. + /// </summary> + [BurstCompile] + public struct JobMergeRaycastCollisionHits : IJob { + [ReadOnly] + public NativeArray<RaycastHit> hit1; + + [ReadOnly] + public NativeArray<RaycastHit> hit2; + + [WriteOnly] + public NativeArray<bool> result; + + public void Execute () { + for (int i = 0; i < hit1.Length; i++) { + result[i] = hit1[i].normal == Vector3.zero && hit2[i].normal == Vector3.zero; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs.meta new file mode 100644 index 0000000..73957ca --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: be7b697c92431a74a8db8ae716c3f0b6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs new file mode 100644 index 0000000..e9bdaf3 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs @@ -0,0 +1,36 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Calculates the default node positions for a grid graph. + /// + /// The node positions will lie on the base plane of the grid graph. + /// + /// See: <see cref="GridGraph.CalculateTransform"/> + /// </summary> + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobNodeGridLayout : IJob, GridIterationUtilities.ICellAction { + public Matrix4x4 graphToWorld; + public IntBounds bounds; + + [WriteOnly] + public NativeArray<Vector3> nodePositions; + + public static Vector3 NodePosition (Matrix4x4 graphToWorld, int x, int z) { + return graphToWorld.MultiplyPoint3x4(new Vector3(x + 0.5f, 0, z + 0.5f)); + } + + public void Execute () { + GridIterationUtilities.ForEachCellIn3DArray(bounds.size, ref this); + } + + public void Execute (uint innerIndex, int x, int y, int z) { + nodePositions[(int)innerIndex] = NodePosition(graphToWorld, x + bounds.min.x, z + bounds.min.z); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs.meta new file mode 100644 index 0000000..4a82431 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7dba760f9d6d81e458c5122c0d28b2df +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs new file mode 100644 index 0000000..7629df7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs @@ -0,0 +1,73 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary>Calculates for each grid node if it should be walkable or not</summary> + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobNodeWalkability : IJob { + /// <summary> + /// If true, use the normal of the raycast hit to check if the ground is flat enough to stand on. + /// + /// Any nodes with a steeper slope than <see cref="maxSlope"/> will be made unwalkable. + /// </summary> + public bool useRaycastNormal; + /// <summary>Max slope in degrees</summary> + public float maxSlope; + /// <summary>Normalized up direction of the graph</summary> + public Vector3 up; + /// <summary>If true, nodes will be made unwalkable if no ground was found under them</summary> + public bool unwalkableWhenNoGround; + /// <summary>For layered grid graphs, if there's a node above another node closer than this distance, the lower node will be made unwalkable</summary> + public float characterHeight; + /// <summary>Number of nodes in each layer</summary> + public int layerStride; + + [ReadOnly] + public NativeArray<float3> nodePositions; + + public NativeArray<float4> nodeNormals; + + [WriteOnly] + public NativeArray<bool> nodeWalkable; + + public void Execute () { + // Cosinus of the max slope + float cosMaxSlopeAngle = math.cos(math.radians(maxSlope)); + float4 upNative = new float4(up.x, up.y, up.z, 0); + float3 upNative3 = upNative.xyz; + + for (int i = 0; i < nodeNormals.Length; i++) { + // walkable will be set to false if no ground was found (unless that setting has been disabled) + // The normal will only be non-zero if something was hit. + bool didHit = math.any(nodeNormals[i]); + var walkable = didHit; + if (!didHit && !unwalkableWhenNoGround && i < layerStride) { + walkable = true; + // If there was no hit, but we still want to make the node walkable, then we set the normal to the up direction + nodeNormals[i] = upNative; + } + + // Check if the node is on a slope steeper than permitted + if (walkable && useRaycastNormal && didHit) { + // Take the dot product to find out the cosine of the angle it has (faster than Vector3.Angle) + float angle = math.dot(nodeNormals[i], upNative); + + // Check if the ground is flat enough to stand on + if (angle < cosMaxSlopeAngle) { + walkable = false; + } + } + + // Check if there is a node above this one (layered grid graph only) + if (walkable && i + layerStride < nodeNormals.Length && math.any(nodeNormals[i + layerStride])) { + walkable = math.dot(upNative3, nodePositions[i + layerStride] - nodePositions[i]) >= characterHeight; + } + + nodeWalkable[i] = walkable; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs.meta new file mode 100644 index 0000000..2a3d418 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a4ace8275be3d2f4b990df3672acb302 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs new file mode 100644 index 0000000..4644787 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs @@ -0,0 +1,45 @@ +#if UNITY_2022_2_OR_NEWER +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Collections.LowLevel.Unsafe; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Prepares a set of capsule commands for collision checking in a grid graph. + /// + /// See: <see cref="GraphCollision"/> + /// </summary> + [BurstCompile] + public struct JobPrepareCapsuleCommands : IJob { + public Vector3 direction; + public Vector3 originOffset; + public float radius; + public LayerMask mask; + public PhysicsScene physicsScene; + + [ReadOnly] + public NativeArray<Vector3> origins; + + [WriteOnly] + public NativeArray<OverlapCapsuleCommand> commands; + + public void Execute () { + var commandSpan = commands.AsUnsafeSpan(); + // It turns out it is faster to set all commands to the same value using MemCpyReplicate and then patch point0 and point1, + // rather than setting each command individually (about 30% faster even). + // MemCpy is very fast. + var queryParameters = new QueryParameters(mask, false, QueryTriggerInteraction.Ignore, false); + commandSpan.Fill(new OverlapCapsuleCommand(physicsScene, Vector3.zero, Vector3.zero, radius, queryParameters)); + + for (int i = 0; i < commandSpan.Length; i++) { + var origin = origins[i] + originOffset; + commandSpan[i].point0 = origin; + commandSpan[i].point1 = origin + direction; + } + } + } +} +#endif diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs.meta new file mode 100644 index 0000000..414cc20 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: af594897c5b7f094585ba27f8be57d45 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs new file mode 100644 index 0000000..e05347d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs @@ -0,0 +1,61 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Collections.LowLevel.Unsafe; +using Pathfinding.Util; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Prepares a set of raycast commands for a grid graph. + /// + /// Each ray will start at <see cref="raycastOffset"/> from the node's position. The end point of the raycast will be the start point + <see cref="raycastDirection"/>. + /// + /// See: <see cref="GraphCollision"/> + /// </summary> + [BurstCompile] + public struct JobPrepareGridRaycast : IJob { + public Matrix4x4 graphToWorld; + public IntBounds bounds; + public Vector3 raycastOffset; + public Vector3 raycastDirection; + public LayerMask raycastMask; + public PhysicsScene physicsScene; + + [WriteOnly] + public NativeArray<RaycastCommand> raycastCommands; + + public void Execute () { + float raycastLength = raycastDirection.magnitude; + var size = bounds.size; + + // In particular Unity 2022.2 seems to assert that RaycastCommands use normalized directions + var direction = raycastDirection.normalized; + var commands = raycastCommands.AsUnsafeSpan(); + + Assert.AreEqual(commands.Length, size.x * size.z); + +#if UNITY_2022_2_OR_NEWER + var queryParameters = new QueryParameters(raycastMask, false, QueryTriggerInteraction.Ignore, false); + // This is about 30% faster than setting each command individually. MemCpy is fast. + commands.Fill(new RaycastCommand(physicsScene, Vector3.zero, direction, queryParameters, raycastLength)); +#else + const int RaycastMaxHits = 1; +#endif + + for (int z = 0; z < size.z; z++) { + int zw = z * size.x; + for (int x = 0; x < size.x; x++) { + int idx = zw + x; + var pos = JobNodeGridLayout.NodePosition(graphToWorld, x + bounds.min.x, z + bounds.min.z); +#if UNITY_2022_2_OR_NEWER + commands[idx].from = pos + raycastOffset; +#else + commands[idx] = new RaycastCommand(pos + raycastOffset, direction, raycastLength, raycastMask, RaycastMaxHits); +#endif + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs.meta new file mode 100644 index 0000000..3800b46 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9f081267224f64d44a99b9962d3d300a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs new file mode 100644 index 0000000..8a7fc7a --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs @@ -0,0 +1,51 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Collections.LowLevel.Unsafe; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Prepares a set of raycast commands for a grid graph. + /// + /// This is very similar to <see cref="JobPrepareGridRaycast"/> but it uses an array of origin points instead of a grid pattern. + /// + /// See: <see cref="GraphCollision"/> + /// </summary> + [BurstCompile] + public struct JobPrepareRaycasts : IJob { + public Vector3 direction; + public Vector3 originOffset; + public float distance; + public LayerMask mask; + public PhysicsScene physicsScene; + + [ReadOnly] + public NativeArray<Vector3> origins; + + [WriteOnly] + public NativeArray<RaycastCommand> raycastCommands; + + public void Execute () { + // In particular Unity 2022.2 seems to assert that RaycastCommands use normalized directions + var direction = this.direction.normalized; + var commands = raycastCommands.AsUnsafeSpan(); + +#if UNITY_2022_2_OR_NEWER + var queryParameters = new QueryParameters(mask, false, QueryTriggerInteraction.Ignore, false); + var defaultCommand = new RaycastCommand(physicsScene, Vector3.zero, direction, queryParameters, distance); + // This is about 30% faster than setting each command individually. MemCpy is fast. + commands.Fill(defaultCommand); +#endif + + for (int i = 0; i < raycastCommands.Length; i++) { +#if UNITY_2022_2_OR_NEWER + commands[i].from = origins[i] + originOffset; +#else + raycastCommands[i] = new RaycastCommand(origins[i] + originOffset, direction, distance, mask, 1); +#endif + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs.meta new file mode 100644 index 0000000..c998e6b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 41ad6b54690bf4949a2974c3ca5d2503 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs new file mode 100644 index 0000000..50562a2 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs @@ -0,0 +1,42 @@ +#if UNITY_2022_2_OR_NEWER +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Collections.LowLevel.Unsafe; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Prepares a set of sphere commands for collision checking in a grid graph. + /// + /// See: <see cref="GraphCollision"/> + /// </summary> + [BurstCompile] + public struct JobPrepareSphereCommands : IJob { + public Vector3 originOffset; + public float radius; + public LayerMask mask; + public PhysicsScene physicsScene; + + [ReadOnly] + public NativeArray<Vector3> origins; + + [WriteOnly] + public NativeArray<OverlapSphereCommand> commands; + + public void Execute () { + var commandSpan = commands.AsUnsafeSpan(); + // It turns out it is faster to set all commands to the same value using MemCpyReplicate and then patch point, + // rather than setting each command individually. + var queryParameters = new QueryParameters(mask, false, QueryTriggerInteraction.Ignore, false); + commandSpan.Fill(new OverlapSphereCommand(physicsScene, Vector3.zero, radius, queryParameters)); + + for (int i = 0; i < commandSpan.Length; i++) { + var origin = origins[i] + originOffset; + commandSpan[i].point = origin; + } + } + } +} +#endif diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs.meta new file mode 100644 index 0000000..f5fbad1 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: db154518b4b8b044d946bb6a9770bdbf +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs new file mode 100644 index 0000000..fd34a1d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs @@ -0,0 +1,121 @@ +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using UnityEngine; +using Pathfinding.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Jobs { + public struct JobRaycastAll { + int maxHits; + public readonly float minStep; + + NativeArray<RaycastHit> results; + NativeArray<RaycastHit> semiResults; + NativeArray<RaycastCommand> commands; + public PhysicsScene physicsScene; + + [BurstCompile] + private struct JobCreateCommands : IJobParallelFor { + public NativeArray<RaycastCommand> commands; + [ReadOnly] + public NativeArray<RaycastHit> raycastHits; + + public float minStep; + public PhysicsScene physicsScene; + + public void Execute (int index) { + var rayHit = raycastHits[index]; + + if (rayHit.normal != default(Vector3)) { + var previousCommand = commands[index]; + // Little hack to bypass same collider hit in specific cases + var point = rayHit.point + previousCommand.direction.normalized * minStep; + var distance = previousCommand.distance - (point - previousCommand.from).magnitude; +#if UNITY_2022_2_OR_NEWER + // TODO: In 2022.2 with the 'hit multiple faces' option, this whole class might be redundant. + var queryParameters = new QueryParameters(previousCommand.queryParameters.layerMask, false, QueryTriggerInteraction.Ignore, false); + commands[index] = new RaycastCommand(physicsScene, point, previousCommand.direction, queryParameters, distance); +#else + commands[index] = new RaycastCommand(point, previousCommand.direction, distance, previousCommand.layerMask, 1); +#endif + } else { +#if UNITY_2022_2_OR_NEWER + // Note: Using a default RaycastCommand may cause Unity to crash. + // This seems to be primarily because it assumes a non-zero direction. + commands[index] = new RaycastCommand(physicsScene, Vector3.zero, Vector3.up, new QueryParameters(0, false, QueryTriggerInteraction.Ignore, false), 1); +#else + commands[index] = new RaycastCommand(Vector3.zero, Vector3.up, 1, 0, 1); +#endif + } + } + } + + [BurstCompile] + private struct JobCombineResults : IJob { + public int maxHits; + [ReadOnly] + public NativeArray<RaycastHit> semiResults; + public NativeArray<RaycastHit> results; + + public void Execute () { + int layerStride = semiResults.Length / maxHits; + + for (int i = 0; i < layerStride; i++) { + int layerOffset = 0; + + for (int j = maxHits - 1; j >= 0; j--) { + if (math.any(semiResults[i + j*layerStride].normal)) { + results[i + layerOffset] = semiResults[i + j*layerStride]; + layerOffset += layerStride; + } + } + } + } + } + + /// <summary>Jobified version of Physics.RaycastNonAlloc.</summary> + /// <param name="commands">Array of commands to perform.</param> + /// <param name="results">Array to store results in.</param> + /// <param name="physicsScene">PhysicsScene to use for the raycasts. Only used in Unity 2022.2 or later.</param> + /// <param name="maxHits">Max hits count per command.</param> + /// <param name="allocator">Allocator to use for the results array.</param> + /// <param name="dependencyTracker">Tracker to use for dependencies.</param> + /// <param name="minStep">Minimal distance each Raycast should progress.</param> + public JobRaycastAll(NativeArray<RaycastCommand> commands, NativeArray<RaycastHit> results, PhysicsScene physicsScene, int maxHits, Allocator allocator, JobDependencyTracker dependencyTracker, float minStep = 0.0001f) { + if (maxHits <= 0) throw new System.ArgumentException("maxHits should be greater than zero"); + if (results.Length < commands.Length * maxHits) throw new System.ArgumentException("Results array length does not match maxHits count"); + if (minStep < 0f) throw new System.ArgumentException("minStep should be more or equal to zero"); + + this.results = results; + this.maxHits = maxHits; + this.minStep = minStep; + this.commands = commands; + this.physicsScene = physicsScene; + + semiResults = dependencyTracker.NewNativeArray<RaycastHit>(maxHits * commands.Length, allocator); + } + + public JobHandle Schedule (JobHandle dependency) { + for (int i = 0; i < maxHits; i++) { + var semiResultsPart = semiResults.GetSubArray(i*commands.Length, commands.Length); + dependency = RaycastCommand.ScheduleBatch(commands, semiResultsPart, 128, dependency); + if (i < maxHits - 1) { + var filter = new JobCreateCommands { + commands = commands, + raycastHits = semiResultsPart, + minStep = minStep, + physicsScene = physicsScene, + }; + dependency = filter.Schedule(commands.Length, 256, dependency); + } + } + + return new JobCombineResults { + semiResults = semiResults, + maxHits = maxHits, + results = results + }.Schedule(dependency); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs.meta new file mode 100644 index 0000000..aed1b42 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9f8eb8ba655e2d2dc88958d073ff52f3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs new file mode 100644 index 0000000..a94b652 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs @@ -0,0 +1,93 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Mathematics; +using Pathfinding.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Reads node data from managed <see cref="GridNodeBase"/> objects into unmanaged arrays. + /// + /// This is done so that burst jobs can later access this data directly. + /// + /// Later, data will be written back to the managed objects using the <see cref="JobWriteNodeData"/> job. + /// </summary> + public struct JobReadNodeData : IJobParallelForBatched { + public System.Runtime.InteropServices.GCHandle nodesHandle; + public uint graphIndex; + + public Slice3D slice; + + [WriteOnly] + public NativeArray<Vector3> nodePositions; + + [WriteOnly] + public NativeArray<uint> nodePenalties; + + [WriteOnly] + public NativeArray<int> nodeTags; + + [WriteOnly] + public NativeArray<ulong> nodeConnections; + + [WriteOnly] + public NativeArray<bool> nodeWalkableWithErosion; + + [WriteOnly] + public NativeArray<bool> nodeWalkable; + + public bool allowBoundsChecks => false; + + struct Reader : GridIterationUtilities.ISliceAction { + public GridNodeBase[] nodes; + public NativeArray<Vector3> nodePositions; + public NativeArray<uint> nodePenalties; + public NativeArray<int> nodeTags; + public NativeArray<ulong> nodeConnections; + public NativeArray<bool> nodeWalkableWithErosion; + public NativeArray<bool> nodeWalkable; + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public void Execute (uint outerIdx, uint innerIdx) { + var dataIdx = (int)innerIdx; + // The data bounds may have more layers than the existing nodes if a new layer is being added. + // We can only copy from the nodes that exist. + if (outerIdx < nodes.Length) { + var node = nodes[outerIdx]; + if (node != null) { + nodePositions[dataIdx] = (Vector3)node.position; + nodePenalties[dataIdx] = node.Penalty; + nodeTags[dataIdx] = (int)node.Tag; + nodeConnections[dataIdx] = node is GridNode gn ? (ulong)gn.GetAllConnectionInternal() : (node as LevelGridNode).gridConnections; + nodeWalkableWithErosion[dataIdx] = node.Walkable; + nodeWalkable[dataIdx] = node.WalkableErosion; + return; + } + } + + // Fallback in case the node was null (only happens for layered grid graphs), + // or if we are adding more layers to the graph, in which case we are outside + // the bounds of the nodes array. + nodePositions[dataIdx] = Vector3.zero; + nodePenalties[dataIdx] = 0; + nodeTags[dataIdx] = 0; + nodeConnections[dataIdx] = 0; + nodeWalkableWithErosion[dataIdx] = false; + nodeWalkable[dataIdx] = false; + } + } + + public void Execute (int startIndex, int count) { + var reader = new Reader { + // This is a managed type, we need to trick Unity to allow this inside of a job + nodes = (GridNodeBase[])nodesHandle.Target, + nodePositions = nodePositions, + nodePenalties = nodePenalties, + nodeTags = nodeTags, + nodeConnections = nodeConnections, + nodeWalkableWithErosion = nodeWalkableWithErosion, + nodeWalkable = nodeWalkable + }; + GridIterationUtilities.ForEachCellIn3DSlice(slice, ref reader); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs.meta new file mode 100644 index 0000000..2092692 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 203c976aeb6f3d84caeab084738a53c8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs new file mode 100644 index 0000000..96a58a8 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs @@ -0,0 +1,91 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Mathematics; +using Pathfinding.Jobs; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// <summary> + /// Writes node data from unmanaged arrays into managed <see cref="GridNodeBase"/> objects. + /// + /// This is done after burst jobs have been working on graph data, as they cannot access the managed objects directly. + /// + /// Earlier, data will have been either calculated from scratch, or read from the managed objects using the <see cref="JobReadNodeData"/> job. + /// </summary> + public struct JobWriteNodeData : IJobParallelForBatched { + public System.Runtime.InteropServices.GCHandle nodesHandle; + public uint graphIndex; + + /// <summary>(width, depth) of the array that the <see cref="nodesHandle"/> refers to</summary> + public int3 nodeArrayBounds; + public IntBounds dataBounds; + public IntBounds writeMask; + + [ReadOnly] + public NativeArray<Vector3> nodePositions; + + [ReadOnly] + public NativeArray<uint> nodePenalties; + + [ReadOnly] + public NativeArray<int> nodeTags; + + [ReadOnly] + public NativeArray<ulong> nodeConnections; + + [ReadOnly] + public NativeArray<bool> nodeWalkableWithErosion; + + [ReadOnly] + public NativeArray<bool> nodeWalkable; + + public bool allowBoundsChecks => false; + + public void Execute (int startIndex, int count) { + // This is a managed type, we need to trick Unity to allow this inside of a job + var nodes = (GridNodeBase[])nodesHandle.Target; + + var relativeMask = writeMask.Offset(-dataBounds.min); + + // Determinstically convert the indices to rows. It is much easier to process a number of whole rows. + var writeSize = writeMask.size; + var zstart = startIndex / (writeSize.x*writeSize.y); + var zend = (startIndex+count) / (writeSize.x*writeSize.y); + + Assert.IsTrue(zstart >= 0 && zstart <= writeSize.z); + Assert.IsTrue(zend >= 0 && zend <= writeSize.z); + relativeMask.min.z = writeMask.min.z + zstart - dataBounds.min.z; + relativeMask.max.z = writeMask.min.z + zend - dataBounds.min.z; + + var dataSize = dataBounds.size; + for (int y = relativeMask.min.y; y < relativeMask.max.y; y++) { + for (int z = relativeMask.min.z; z < relativeMask.max.z; z++) { + var rowOffset1 = (y*dataSize.z + z)*dataSize.x; + var rowOffset2 = (z + dataBounds.min.z)*nodeArrayBounds.x + dataBounds.min.x; + var rowOffset3 = (y + dataBounds.min.y)*nodeArrayBounds.z*nodeArrayBounds.x + rowOffset2; + for (int x = relativeMask.min.x; x < relativeMask.max.x; x++) { + int dataIndex = rowOffset1 + x; + int nodeIndex = rowOffset3 + x; + var node = nodes[nodeIndex]; + if (node != null) { + node.GraphIndex = graphIndex; + node.NodeInGridIndex = rowOffset2 + x; + // TODO: Use UnsafeSpan + node.position = (Int3)nodePositions[dataIndex]; + node.Penalty = nodePenalties[dataIndex]; + node.Tag = (uint)nodeTags[dataIndex]; + if (node is GridNode gridNode) { + gridNode.SetAllConnectionInternal((int)nodeConnections[dataIndex]); + } else if (node is LevelGridNode levelGridNode) { + levelGridNode.LayerCoordinateInGrid = y + dataBounds.min.y; + levelGridNode.SetAllConnectionInternal(nodeConnections[dataIndex]); + } + node.Walkable = nodeWalkableWithErosion[dataIndex]; + node.WalkableErosion = nodeWalkable[dataIndex]; + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs.meta new file mode 100644 index 0000000..503743b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 86fcb7ce1d754024290197052acf39d5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules.meta new file mode 100644 index 0000000..ab73ead --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e96df07c8616c534c99a64575f066a3d +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs new file mode 100644 index 0000000..174b7d2 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs @@ -0,0 +1,293 @@ +using System.Collections.Generic; + +namespace Pathfinding.Graphs.Grid.Rules { + using Pathfinding.Serialization; + using Pathfinding.Jobs; + using Unity.Jobs; + using Unity.Collections; + using Unity.Mathematics; + + public class CustomGridGraphRuleEditorAttribute : System.Attribute { + public System.Type type; + public string name; + public CustomGridGraphRuleEditorAttribute(System.Type type, string name) { + this.type = type; + this.name = name; + } + } + + /// <summary> + /// Container for all rules in a grid graph. + /// + /// <code> + /// // Get the first grid graph in the scene + /// var gridGraph = AstarPath.active.data.gridGraph; + /// + /// gridGraph.rules.AddRule(new Pathfinding.Graphs.Grid.Rules.RuleAnglePenalty { + /// penaltyScale = 10000, + /// curve = AnimationCurve.Linear(0, 0, 90, 1), + /// }); + /// </code> + /// + /// See: <see cref="Pathfinding.GridGraph.rules"/> + /// See: grid-rules (view in online documentation for working links) + /// </summary> + [JsonOptIn] + public class GridGraphRules { + List<System.Action<Context> >[] jobSystemCallbacks; + List<System.Action<Context> >[] mainThreadCallbacks; + + /// <summary>List of all rules</summary> + [JsonMember] + List<GridGraphRule> rules = new List<GridGraphRule>(); + + long lastHash; + + /// <summary>Context for when scanning or updating a graph</summary> + public class Context { + /// <summary>Graph which is being scanned or updated</summary> + public GridGraph graph; + /// <summary>Data for all the nodes as NativeArrays</summary> + public GridGraphScanData data; + /// <summary> + /// Tracks dependencies between jobs to allow parallelism without tediously specifying dependencies manually. + /// Always use when scheduling jobs. + /// </summary> + public JobDependencyTracker tracker => data.dependencyTracker; + } + + public void AddRule (GridGraphRule rule) { + rules.Add(rule); + lastHash = -1; + } + + public void RemoveRule (GridGraphRule rule) { + rules.Remove(rule); + lastHash = -1; + } + + public IReadOnlyList<GridGraphRule> GetRules () { + if (rules == null) rules = new List<GridGraphRule>(); + return rules.AsReadOnly(); + } + + long Hash () { + long hash = 196613; + + for (int i = 0; i < rules.Count; i++) { + if (rules[i] != null && rules[i].enabled) hash = hash * 1572869 ^ (long)rules[i].Hash; + } + return hash; + } + + public void RebuildIfNecessary () { + var newHash = Hash(); + + if (newHash == lastHash && jobSystemCallbacks != null && mainThreadCallbacks != null) return; + lastHash = newHash; + Rebuild(); + } + + public void Rebuild () { + rules = rules ?? new List<GridGraphRule>(); + jobSystemCallbacks = jobSystemCallbacks ?? new List<System.Action<Context> >[6]; + for (int i = 0; i < jobSystemCallbacks.Length; i++) { + if (jobSystemCallbacks[i] != null) jobSystemCallbacks[i].Clear(); + } + mainThreadCallbacks = mainThreadCallbacks ?? new List<System.Action<Context> >[6]; + for (int i = 0; i < mainThreadCallbacks.Length; i++) { + if (mainThreadCallbacks[i] != null) mainThreadCallbacks[i].Clear(); + } + for (int i = 0; i < rules.Count; i++) { + if (rules[i].enabled) rules[i].Register(this); + } + } + + public void DisposeUnmanagedData () { + if (rules != null) { + for (int i = 0; i < rules.Count; i++) { + if (rules[i] != null) { + rules[i].DisposeUnmanagedData(); + rules[i].SetDirty(); + } + } + } + } + + static void CallActions (List<System.Action<Context> > actions, Context context) { + if (actions != null) { + try { + for (int i = 0; i < actions.Count; i++) actions[i](context); + } catch (System.Exception e) { + UnityEngine.Debug.LogException(e); + } + } + } + + /// <summary> + /// Executes the rules for the given pass. + /// Call handle.Complete on, or wait for, all yielded job handles. + /// </summary> + public IEnumerator<JobHandle> ExecuteRule (GridGraphRule.Pass rule, Context context) { + if (jobSystemCallbacks == null) Rebuild(); + CallActions(jobSystemCallbacks[(int)rule], context); + + if (mainThreadCallbacks[(int)rule] != null && mainThreadCallbacks[(int)rule].Count > 0) { + if (!context.tracker.forceLinearDependencies) yield return context.tracker.AllWritesDependency; + CallActions(mainThreadCallbacks[(int)rule], context); + } + } + + public void ExecuteRuleMainThread (GridGraphRule.Pass rule, Context context) { + if (jobSystemCallbacks[(int)rule] != null && jobSystemCallbacks[(int)rule].Count > 0) throw new System.Exception("A job system pass has been added for the " + rule + " pass. " + rule + " only supports main thread callbacks."); + if (context.tracker != null) context.tracker.AllWritesDependency.Complete(); + CallActions(mainThreadCallbacks[(int)rule], context); + } + + /// <summary> + /// Adds a pass callback that uses the job system. + /// This rule should only schedule jobs using the `Context.tracker` dependency tracker. Data is not safe to access directly in the callback + /// + /// This method should only be called from rules in their Register method. + /// </summary> + public void AddJobSystemPass (GridGraphRule.Pass pass, System.Action<Context> action) { + var index = (int)pass; + + if (jobSystemCallbacks[index] == null) { + jobSystemCallbacks[index] = new List<System.Action<Context> >(); + } + jobSystemCallbacks[index].Add(action); + } + + /// <summary> + /// Adds a pass callback that runs in the main thread. + /// The callback may access and modify any data in the context. + /// You do not need to schedule jobs in order to access the data. + /// + /// Warning: Not all data in the Context is valid for every pass. For example you cannot access node connections in the BeforeConnections pass + /// since they haven't been calculated yet. + /// + /// This is a bit slower than <see cref="AddJobSystemPass"/> since parallelism and the burst compiler cannot be used. + /// But if you need to use non-thread-safe APIs or data then this is a good choice. + /// + /// This method should only be called from rules in their Register method. + /// </summary> + public void AddMainThreadPass (GridGraphRule.Pass pass, System.Action<Context> action) { + var index = (int)pass; + + if (mainThreadCallbacks[index] == null) { + mainThreadCallbacks[index] = new List<System.Action<Context> >(); + } + mainThreadCallbacks[index].Add(action); + } + + /// <summary>Deprecated: Use AddJobSystemPass or AddMainThreadPass instead</summary> + [System.Obsolete("Use AddJobSystemPass or AddMainThreadPass instead")] + public void Add (GridGraphRule.Pass pass, System.Action<Context> action) { + AddJobSystemPass(pass, action); + } + } + + /// <summary> + /// Custom rule for a grid graph. + /// See: <see cref="GridGraphRules"/> + /// See: grid-rules (view in online documentation for working links) + /// </summary> + [JsonDynamicType] + // Compatibility with old versions + [JsonDynamicTypeAlias("Pathfinding.RuleTexture", typeof(RuleTexture))] + [JsonDynamicTypeAlias("Pathfinding.RuleAnglePenalty", typeof(RuleAnglePenalty))] + [JsonDynamicTypeAlias("Pathfinding.RuleElevationPenalty", typeof(RuleElevationPenalty))] + [JsonDynamicTypeAlias("Pathfinding.RulePerLayerModifications", typeof(RulePerLayerModifications))] + public abstract class GridGraphRule { + /// <summary>Only enabled rules are executed</summary> + [JsonMember] + public bool enabled = true; + int dirty = 1; + + /// <summary> + /// Where in the scanning process a rule will be executed. + /// Check the documentation for <see cref="GridGraphScanData"/> to see which data fields are valid in which passes. + /// </summary> + public enum Pass { + /// <summary> + /// Before the collision testing phase but after height testing. + /// This is very early. Most data is not valid by this point. + /// + /// You can use this if you want to modify the node positions and still have it picked up by the collision testing code. + /// </summary> + BeforeCollision, + /// <summary> + /// Before connections are calculated. + /// At this point height testing and collision testing has been done (if they are enabled). + /// + /// This is the most common pass to use. + /// If you are modifying walkability here then connections and erosion will be calculated properly. + /// </summary> + BeforeConnections, + /// <summary> + /// After connections are calculated. + /// + /// If you are modifying connections directly you should do that in this pass. + /// + /// Note: If erosion is used then this pass will be executed twice. One time before erosion and one time after erosion + /// when the connections are calculated again. + /// </summary> + AfterConnections, + /// <summary> + /// After erosion is calculated but before connections have been recalculated. + /// + /// If no erosion is used then this pass will not be executed. + /// </summary> + AfterErosion, + /// <summary> + /// After everything else. + /// This pass is executed after everything else is done. + /// You should not modify walkability in this pass because then the node connections will not be up to date. + /// </summary> + PostProcess, + /// <summary> + /// After the graph update has been applied to the graph. + /// + /// This pass can only be added as a main-thread pass. + /// + /// Warning: No native data in the context is valid at this point. It has all been disposed. + /// You cannot modify any data in this pass. + /// </summary> + AfterApplied, + } + + /// <summary> + /// Hash of the settings for this rule. + /// The <see cref="Register"/> method will be called again whenever the hash changes. + /// If the hash does not change it is assumed that the <see cref="Register"/> method does not need to be called again. + /// </summary> + public virtual int Hash => dirty; + + /// <summary> + /// Call if you have changed any setting of the rule. + /// This will ensure that any cached data the rule uses is rebuilt. + /// If you do not do this then any settings changes may not affect the graph when it is rescanned or updated. + /// + /// The purpose of this method call is to cause the <see cref="Hash"/> property to change. If your custom rule overrides the Hash property to + /// return a hash of some settings, then you do not need to call this method for the changes the hash function already accounts for. + /// </summary> + public virtual void SetDirty () { + dirty++; + } + + /// <summary> + /// Called when the rule is removed or the graph is destroyed. + /// Use this to e.g. clean up any NativeArrays that the rule uses. + /// + /// Note: The rule should remain valid after this method has been called. + /// However the <see cref="Register"/> method is guaranteed to be called before the rule is executed again. + /// </summary> + public virtual void DisposeUnmanagedData () { + } + + /// <summary>Does preprocessing and adds callbacks to the <see cref="GridGraphRules"/> object</summary> + public virtual void Register (GridGraphRules rules) { + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs.meta new file mode 100644 index 0000000..a73e50c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4d90c9a7bca49464796933f43b5506fc +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs new file mode 100644 index 0000000..1322770 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs @@ -0,0 +1,81 @@ +namespace Pathfinding.Graphs.Grid.Rules { + using Pathfinding.Jobs; + using Unity.Jobs; + using Unity.Collections; + using Unity.Burst; + using UnityEngine; + using Unity.Mathematics; + + /// <summary> + /// Applies penalty based on the slope of the surface below the node. + /// + /// This is useful if you for example want to discourage agents from walking on steep slopes. + /// + /// The penalty applied is equivalent to: + /// + /// <code> + /// penalty = curve.evaluate(slope angle in degrees) * penaltyScale + /// </code> + /// + /// [Open online documentation to see images] + /// + /// See: grid-rules (view in online documentation for working links) + /// </summary> + [Pathfinding.Util.Preserve] + public class RuleAnglePenalty : GridGraphRule { + public float penaltyScale = 10000; + public AnimationCurve curve = AnimationCurve.Linear(0, 0, 90, 1); + NativeArray<float> angleToPenalty; + + public override void Register (GridGraphRules rules) { + if (!angleToPenalty.IsCreated) angleToPenalty = new NativeArray<float>(32, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < angleToPenalty.Length; i++) { + angleToPenalty[i] = Mathf.Max(0, curve.Evaluate(90.0f * i / (angleToPenalty.Length - 1)) * penaltyScale); + } + + rules.AddJobSystemPass(Pass.BeforeConnections, context => { + new JobPenaltyAngle { + angleToPenalty = angleToPenalty, + up = context.data.up, + nodeNormals = context.data.nodes.normals, + penalty = context.data.nodes.penalties, + }.Schedule(context.tracker); + }); + } + + public override void DisposeUnmanagedData () { + if (angleToPenalty.IsCreated) angleToPenalty.Dispose(); + } + + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobPenaltyAngle : IJob { + public Vector3 up; + + [ReadOnly] + public NativeArray<float> angleToPenalty; + + [ReadOnly] + public NativeArray<float4> nodeNormals; + + public NativeArray<uint> penalty; + + public void Execute () { + float4 up = new float4(this.up.x, this.up.y, this.up.z, 0); + + for (int i = 0; i < penalty.Length; i++) { + float4 normal = nodeNormals[i]; + if (math.any(normal)) { + float angle = math.acos(math.dot(normal, up)); + // Take the dot product to find out the cosinus of the angle it has + // Add penalty based on the angle from a precalculated array + float x = angle*(angleToPenalty.Length - 1)/math.PI; + int ix = (int)x; + float p1 = angleToPenalty[math.max(ix, 0)]; + float p2 = angleToPenalty[math.min(ix + 1, angleToPenalty.Length - 1)]; + penalty[i] += (uint)math.lerp(p1, p2, x - ix); + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs.meta new file mode 100644 index 0000000..d047088 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 40d5c4aeb2276457f8fe040e4c5d71fe +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs new file mode 100644 index 0000000..6f62660 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs @@ -0,0 +1,76 @@ +namespace Pathfinding.Graphs.Grid.Rules { + using Pathfinding.Jobs; + using Unity.Jobs; + using Unity.Collections; + using Unity.Burst; + using UnityEngine; + using Unity.Mathematics; + + /// <summary> + /// Applies penalty based on the elevation of the node. + /// + /// This is useful if you for example want to discourage agents from walking high up in mountain regions. + /// + /// The penalty applied is equivalent to: + /// + /// <code> + /// penalty = curve.evaluate(Mathf.Clamp01(Mathf.InverseLerp(lower elevation range, upper elevation range, elevation))) * penaltyScale + /// </code> + /// + /// [Open online documentation to see images] + /// + /// See: grid-rules (view in online documentation for working links) + /// </summary> + [Pathfinding.Util.Preserve] + public class RuleElevationPenalty : GridGraphRule { + public float penaltyScale = 10000; + public Vector2 elevationRange = new Vector2(0, 100); + public AnimationCurve curve = AnimationCurve.Linear(0, 0, 1, 1); + NativeArray<float> elevationToPenalty; + + public override void Register (GridGraphRules rules) { + if (!elevationToPenalty.IsCreated) elevationToPenalty = new NativeArray<float>(64, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < elevationToPenalty.Length; i++) { + elevationToPenalty[i] = Mathf.Max(0, penaltyScale * curve.Evaluate(i * 1.0f / (elevationToPenalty.Length - 1))); + } + + var clampedElevationRange = new Vector2(math.max(0, elevationRange.x), math.max(1, elevationRange.y)); + rules.AddJobSystemPass(Pass.BeforeConnections, context => { + //var elevationRangeScale = Matrix4x4.TRS(new Vector3(0, -clampedElevationRange.x, 0), Quaternion.identity, new Vector3(1, 1/(clampedElevationRange.y - clampedElevationRange.x), 1)); + var elevationRangeScale = Matrix4x4.Scale(new Vector3(1, 1/(clampedElevationRange.y - clampedElevationRange.x), 1)) * Matrix4x4.Translate(new Vector3(0, -clampedElevationRange.x, 0)); + new JobElevationPenalty { + elevationToPenalty = elevationToPenalty, + nodePositions = context.data.nodes.positions, + worldToGraph = elevationRangeScale * context.data.transform.matrix.inverse, + penalty = context.data.nodes.penalties, + }.Schedule(context.tracker); + }); + } + + public override void DisposeUnmanagedData () { + if (elevationToPenalty.IsCreated) elevationToPenalty.Dispose(); + } + + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobElevationPenalty : IJob { + [ReadOnly] + public NativeArray<float> elevationToPenalty; + + [ReadOnly] + public NativeArray<Vector3> nodePositions; + + public Matrix4x4 worldToGraph; + public NativeArray<uint> penalty; + + public void Execute () { + for (int i = 0; i < penalty.Length; i++) { + float y = math.clamp(worldToGraph.MultiplyPoint3x4(nodePositions[i]).y, 0, 1) * (elevationToPenalty.Length - 1); + int iy = (int)y; + float p1 = elevationToPenalty[iy]; + float p2 = elevationToPenalty[math.min(iy + 1, elevationToPenalty.Length - 1)]; + penalty[i] += (uint)math.lerp(p1, p2, y - iy); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs.meta new file mode 100644 index 0000000..5878475 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d2933158d922e49e39a332d795d26d68 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs new file mode 100644 index 0000000..1684b04 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs @@ -0,0 +1,79 @@ +using Pathfinding.Jobs; + +namespace Pathfinding.Graphs.Grid.Rules { + /// <summary> + /// Modifies nodes based on the layer of the surface under the node. + /// + /// You can for example make all surfaces with a specific layer make the nodes get a specific tag. + /// + /// [Open online documentation to see images] + /// + /// See: grid-rules (view in online documentation for working links) + /// </summary> + [Pathfinding.Util.Preserve] + public class RulePerLayerModifications : GridGraphRule { + public PerLayerRule[] layerRules = new PerLayerRule[0]; + const int SetTagBit = 1 << 30; + + public struct PerLayerRule { + /// <summary>Layer this rule applies to</summary> + public int layer; + /// <summary>The action to apply to matching nodes</summary> + public RuleAction action; + /// <summary> + /// Tag for the RuleAction.SetTag action. + /// Must be between 0 and <see cref="Pathfinding.GraphNode.MaxTagIndex"/> + /// </summary> + public int tag; + } + + public enum RuleAction { + /// <summary>Sets the tag of all affected nodes to <see cref="PerLayerRule.tag"/></summary> + SetTag, + /// <summary>Makes all affected nodes unwalkable</summary> + MakeUnwalkable, + } + + public override void Register (GridGraphRules rules) { + int[] layerToTag = new int[32]; + bool[] layerToUnwalkable = new bool[32]; + for (int i = 0; i < layerRules.Length; i++) { + var rule = layerRules[i]; + if (rule.action == RuleAction.SetTag) { + layerToTag[rule.layer] = SetTagBit | rule.tag; + } else { + layerToUnwalkable[rule.layer] = true; + } + } + + rules.AddMainThreadPass(Pass.BeforeConnections, context => { + if (!context.data.heightHits.IsCreated) { + UnityEngine.Debug.LogError("RulePerLayerModifications requires height testing to be enabled on the grid graph", context.graph.active); + return; + } + + var raycastHits = context.data.heightHits; + var nodeWalkable = context.data.nodes.walkable; + var nodeTags = context.data.nodes.tags; + var slice = new Slice3D(context.data.nodes.bounds, context.data.heightHitsBounds); + var size = slice.slice.size; + for (int y = 0; y < size.y; y++) { + for (int z = 0; z < size.z; z++) { + var rowOffset = y * size.x * size.z + z * size.x; + for (int x = 0; x < size.x; x++) { + var innerIndex = rowOffset + x; + var outerIndex = slice.InnerCoordinateToOuterIndex(x, y, z); + var coll = raycastHits[innerIndex].collider; + if (coll != null) { + var layer = coll.gameObject.layer; + if (layerToUnwalkable[layer]) nodeWalkable[outerIndex] = false; + var tag = layerToTag[layer]; + if ((tag & SetTagBit) != 0) nodeTags[outerIndex] = tag & 0xFF; + } + } + } + } + }); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs.meta new file mode 100644 index 0000000..bfa3859 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4869e55551c0e4e1abaaf19bcc3d44a1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs new file mode 100644 index 0000000..ddc6a9c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs @@ -0,0 +1,181 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Grid.Rules { + using Pathfinding.Jobs; + + /// <summary> + /// Modifies nodes based on the contents of a texture. + /// + /// This can be used to "paint" penalties or walkability using an external program such as Photoshop. + /// + /// [Open online documentation to see images] + /// + /// See: grid-rules (view in online documentation for working links) + /// </summary> + [Pathfinding.Util.Preserve] + public class RuleTexture : GridGraphRule { + public Texture2D texture; + + public ChannelUse[] channels = new ChannelUse[4]; + public float[] channelScales = { 1000, 1000, 1000, 1000 }; + + public ScalingMode scalingMode = ScalingMode.StretchToFitGraph; + public float nodesPerPixel = 1; + + NativeArray<int> colors; + + public enum ScalingMode { + FixedScale, + StretchToFitGraph, + } + + public override int Hash { + get { + var h = base.Hash ^ (texture != null ? (int)texture.updateCount : 0); +#if UNITY_EDITOR + if (texture != null) h ^= (int)texture.imageContentsHash.GetHashCode(); +#endif + return h; + } + } + + public enum ChannelUse { + None, + /// <summary>Penalty goes from 0 to channelScale depending on the channel value</summary> + Penalty, + /// <summary>Node Y coordinate goes from 0 to channelScale depending on the channel value</summary> + Position, + /// <summary>If channel value is zero the node is made unwalkable, penalty goes from 0 to channelScale depending on the channel value</summary> + WalkablePenalty, + /// <summary>If channel value is zero the node is made unwalkable</summary> + Walkable, + } + + public override void Register (GridGraphRules rules) { + if (texture == null) return; + + if (!texture.isReadable) { + Debug.LogError("Texture for the texture rule on a grid graph is not marked as readable.", texture); + return; + } + + if (colors.IsCreated) colors.Dispose(); + colors = new NativeArray<Color32>(texture.GetPixels32(), Allocator.Persistent).Reinterpret<int>(); + + // Make sure this is done outside the delegate, just in case the texture is later resized + var textureSize = new int2(texture.width, texture.height); + + float4 channelPenaltiesCombined = float4.zero; + bool4 channelDeterminesWalkability = false; + float4 channelPositionScalesCombined = float4.zero; + for (int i = 0; i < 4; i++) { + channelPenaltiesCombined[i] = channels[i] == ChannelUse.Penalty || channels[i] == ChannelUse.WalkablePenalty ? channelScales[i] : 0; + channelDeterminesWalkability[i] = channels[i] == ChannelUse.Walkable || channels[i] == ChannelUse.WalkablePenalty; + channelPositionScalesCombined[i] = channels[i] == ChannelUse.Position ? channelScales[i] : 0; + } + + channelPositionScalesCombined /= 255.0f; + channelPenaltiesCombined /= 255.0f; + + if (math.any(channelPositionScalesCombined)) { + rules.AddJobSystemPass(Pass.BeforeCollision, context => { + new JobTexturePosition { + colorData = colors, + nodePositions = context.data.nodes.positions, + nodeNormals = context.data.nodes.normals, + bounds = context.data.nodes.bounds, + colorDataSize = textureSize, + scale = scalingMode == ScalingMode.FixedScale ? 1.0f/math.max(0.01f, nodesPerPixel) : textureSize / new float2(context.graph.width, context.graph.depth), + channelPositionScale = channelPositionScalesCombined, + graphToWorld = context.data.transform.matrix, + }.Schedule(context.tracker); + }); + } + + rules.AddJobSystemPass(Pass.BeforeConnections, context => { + new JobTexturePenalty { + colorData = colors, + penalty = context.data.nodes.penalties, + walkable = context.data.nodes.walkable, + nodeNormals = context.data.nodes.normals, + bounds = context.data.nodes.bounds, + colorDataSize = textureSize, + scale = scalingMode == ScalingMode.FixedScale ? 1.0f/math.max(0.01f, nodesPerPixel) : textureSize / new float2(context.graph.width, context.graph.depth), + channelPenalties = channelPenaltiesCombined, + channelDeterminesWalkability = channelDeterminesWalkability, + }.Schedule(context.tracker); + }); + } + + public override void DisposeUnmanagedData () { + if (colors.IsCreated) colors.Dispose(); + } + + [BurstCompile] + public struct JobTexturePosition : IJob, GridIterationUtilities.INodeModifier { + [ReadOnly] + public NativeArray<int> colorData; + [WriteOnly] + public NativeArray<Vector3> nodePositions; + [ReadOnly] + public NativeArray<float4> nodeNormals; + + public Matrix4x4 graphToWorld; + public IntBounds bounds; + public int2 colorDataSize; + public float2 scale; + public float4 channelPositionScale; + + public void ModifyNode (int dataIndex, int dataX, int dataLayer, int dataZ) { + var offset = bounds.min.xz; + int2 colorPos = math.clamp((int2)math.round((new float2(dataX, dataZ) + offset) * scale), int2.zero, colorDataSize - new int2(1, 1)); + int colorIndex = colorPos.y*colorDataSize.x + colorPos.x; + + int4 color = new int4((colorData[colorIndex] >> 0) & 0xFF, (colorData[colorIndex] >> 8) & 0xFF, (colorData[colorIndex] >> 16) & 0xFF, (colorData[colorIndex] >> 24) & 0xFF); + + float y = math.dot(channelPositionScale, color); + + nodePositions[dataIndex] = graphToWorld.MultiplyPoint3x4(new Vector3((bounds.min.x + dataX) + 0.5f, y, (bounds.min.z + dataZ) + 0.5f)); + } + + public void Execute () { + GridIterationUtilities.ForEachNode(bounds.size, nodeNormals, ref this); + } + } + + [BurstCompile] + public struct JobTexturePenalty : IJob, GridIterationUtilities.INodeModifier { + [ReadOnly] + public NativeArray<int> colorData; + public NativeArray<uint> penalty; + public NativeArray<bool> walkable; + [ReadOnly] + public NativeArray<float4> nodeNormals; + + public IntBounds bounds; + public int2 colorDataSize; + public float2 scale; + public float4 channelPenalties; + public bool4 channelDeterminesWalkability; + + public void ModifyNode (int dataIndex, int dataX, int dataLayer, int dataZ) { + var offset = bounds.min.xz; + int2 colorPos = math.clamp((int2)math.round((new float2(dataX, dataZ) + offset) * scale), int2.zero, colorDataSize - new int2(1, 1)); + int colorIndex = colorPos.y*colorDataSize.x + colorPos.x; + + int4 color = new int4((colorData[colorIndex] >> 0) & 0xFF, (colorData[colorIndex] >> 8) & 0xFF, (colorData[colorIndex] >> 16) & 0xFF, (colorData[colorIndex] >> 24) & 0xFF); + + penalty[dataIndex] += (uint)math.dot(channelPenalties, color); + walkable[dataIndex] = walkable[dataIndex] & !math.any(channelDeterminesWalkability & (color == 0)); + } + + public void Execute () { + GridIterationUtilities.ForEachNode(bounds.size, nodeNormals, ref this); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs.meta new file mode 100644 index 0000000..6b3c4aa --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 42c128143490d447fa6420a4f35fe9bb +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: |