From 8722a9920c1f6119bf6e769cba270e63097f8e25 Mon Sep 17 00:00:00 2001 From: chai <215380520@qq.com> Date: Thu, 23 May 2024 10:08:29 +0800 Subject: + astar project --- .../Graphs/Grid/GraphCollision.cs | 387 +++++++++++++++++++++ 1 file changed, 387 insertions(+) create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs') 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; + + /// + /// Handles collision checking for graphs. + /// Mostly used by grid based graphs + /// + [System.Serializable] + public class GraphCollision { + /// + /// Collision shape to use. + /// See: + /// + public ColliderType type = ColliderType.Capsule; + + /// + /// 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 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 . + /// + /// If is set to Ray, this does not affect anything. + /// + /// [Open online documentation to see images] + /// + public float diameter = 1F; + + /// + /// Height of capsule or length of ray when checking for collision. + /// If 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. + /// + public float height = 2F; + + /// + /// 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. + /// + public float collisionOffset; + + /// + /// Direction of the ray when checking for collision. + /// If is not Ray, this does not affect anything + /// + /// Deprecated: Only the Both mode is supported now. + /// + [System.Obsolete("Only the Both mode is supported now")] + public RayDirection rayDirection = RayDirection.Both; + + /// Layers to be treated as obstacles. + public LayerMask mask; + + /// Layers to be included in the height check. + public LayerMask heightMask = -1; + + /// + /// 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] + /// + public float fromHeight = 100; + + /// + /// Toggles thick raycast. + /// See: https://docs.unity3d.com/ScriptReference/Physics.SphereCast.html + /// + public bool thickRaycast; + + /// + /// Diameter of the thick raycast in nodes. + /// 1 equals + /// + public float thickRaycastDiameter = 1; + + /// Make nodes unwalkable when no ground was found with the height raycast. If height raycast is turned off, this doesn't affect anything. + public bool unwalkableWhenNoGround = true; + + /// + /// 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 from 3D versions to their corresponding 2D versions. For example the sphere shape becomes a circle. + /// + /// The setting will be ignored when 2D physics is used. + /// + /// See: http://docs.unity3d.com/ScriptReference/Physics2D.html + /// + public bool use2D; + + /// Toggle collision check + public bool collisionCheck = true; + + /// + /// Toggle height check. If false, the grid will be flat. + /// + /// This setting will be ignored when 2D physics is used. + /// + public bool heightCheck = true; + + /// + /// Direction to use as UP. + /// See: Initialize + /// + public Vector3 up; + + /// + /// * . + /// See: Initialize + /// + private Vector3 upheight; + + /// Used for 2D collision queries + private ContactFilter2D contactFilter; + + /// + /// 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. + /// + private static Collider2D[] dummyArray = new Collider2D[1]; + + /// + /// * scale * 0.5. + /// Where scale usually is + /// See: Initialize + /// + private float finalRadius; + + /// + /// * scale * 0.5. + /// Where scale usually is See: Initialize + /// + private float finalRaycastRadius; + + /// Offset to apply after each raycast to make sure we don't hit the same point again in CheckHeightAll + public const float RaycastErrorMargin = 0.005F; + + /// + /// Sets up several variables using the specified matrix and scale. + /// See: GraphCollision.up + /// See: GraphCollision.upheight + /// See: GraphCollision.finalRadius + /// See: GraphCollision.finalRaycastRadius + /// + 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 }; + } + + /// + /// Returns true if the position is not obstructed. + /// If is false, this will always return true. + /// + 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); + } + } + + /// + /// Returns the position with the correct height. + /// If is false, this will return position. + /// + public Vector3 CheckHeight (Vector3 position) { + RaycastHit hit; + bool walkable; + + return CheckHeight(position, out hit, out walkable); + } + + /// + /// Returns the position with the correct height. + /// If 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 + /// + 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; + } + + /// Internal buffer used by + RaycastHit[] hitBuffer = new RaycastHit[8]; + + /// + /// 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. + /// + 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 + } + + /// + /// 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 + /// + public void JobCollisionRay (NativeArray nodePositions, NativeArray collisionCheckResult, Vector3 up, Allocator allocationMethod, JobDependencyTracker dependencyTracker) { + var collisionRaycastCommands1 = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionRaycastCommands2 = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionHits1 = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionHits2 = dependencyTracker.NewNativeArray(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 nodePositions, NativeArray collisionCheckResult, Vector3 up, Allocator allocationMethod, JobDependencyTracker dependencyTracker) { + var commands = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionHits = dependencyTracker.NewNativeArray(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 nodePositions, NativeArray collisionCheckResult, Vector3 up, Allocator allocationMethod, JobDependencyTracker dependencyTracker) { + var commands = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionHits = dependencyTracker.NewNativeArray(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 + } + + /// + /// Determines collision check shape. + /// See: + /// + public enum ColliderType { + /// Uses a Sphere, Physics.CheckSphere. In 2D this is a circle instead. + Sphere, + /// Uses a Capsule, Physics.CheckCapsule. This will behave identically to the Sphere mode in 2D. + Capsule, + /// Uses a Ray, Physics.Linecast. In 2D this is a single point instead. + Ray + } + + /// Determines collision check ray direction + public enum RayDirection { + Up, /// < Casts the ray from the bottom upwards + Down, /// < Casts the ray from the top downwards + Both /// < Casts two rays in both directions + } +} -- cgit v1.1-26-g67d0