+ astar project

1 files changed, 141 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Behaviors/AIPathAlignedToSurface.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Behaviors/AIPathAlignedToSurface.cs
new file mode 100644
index 0000000..c6a58c6
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Behaviors/AIPathAlignedToSurface.cs
@@ -0,0 +1,141 @@
+using UnityEngine;
+using System.Collections.Generic;
+using UnityEngine.Profiling;
+
+namespace Pathfinding {
+	using Pathfinding.Util;
+	using Unity.Collections.LowLevel.Unsafe;
+
+	/// <summary>
+	/// Movement script for curved worlds.
+	/// This script inherits from AIPath, but adjusts its movement plane every frame using the ground normal.
+	/// </summary>
+	[HelpURL("https://arongranberg.com/astar/documentation/stable/aipathalignedtosurface.html")]
+	public class AIPathAlignedToSurface : AIPath {
+		/// <summary>Scratch dictionary used to avoid allocations every frame</summary>
+		static readonly Dictionary<Mesh, int> scratchDictionary = new Dictionary<Mesh, int>();
+
+		protected override void OnEnable () {
+			base.OnEnable();
+			movementPlane = new Util.SimpleMovementPlane(rotation);
+		}
+
+		protected override void ApplyGravity (float deltaTime) {
+			// Apply gravity
+			if (usingGravity) {
+				// Gravity is relative to the current surface.
+				// Only the normal direction is well defined however so x and z are ignored.
+				verticalVelocity += deltaTime * (float.IsNaN(gravity.x) ? Physics.gravity.y : gravity.y);
+			} else {
+				verticalVelocity = 0;
+			}
+		}
+
+		/// <summary>
+		/// Calculates smoothly interpolated normals for all raycast hits and uses that to set the movement planes of the agents.
+		///
+		/// To support meshes that change at any time, we use Mesh.AcquireReadOnlyMeshData to get a read-only view of the mesh data.
+		/// This is only efficient if we batch all updates and make a single call to Mesh.AcquireReadOnlyMeshData.
+		///
+		/// This method is quite convoluted due to having to read the raw vertex data streams from unity meshes to avoid allocations.
+		/// </summary>
+		public static void UpdateMovementPlanes (AIPathAlignedToSurface[] components, int count) {
+			Profiler.BeginSample("UpdateMovementPlanes");
+			var meshes = ListPool<Mesh>.Claim();
+			var componentsByMesh = new List<List<AIPathAlignedToSurface> >();
+			var meshToIndex = scratchDictionary;
+			for (int i = 0; i < count; i++) {
+				var c = components[i].lastRaycastHit.collider;
+				// triangleIndex can be -1 if the mesh collider is convex, and the raycast started inside it.
+				// This is not a documented behavior, but it seems to happen in practice.
+				if (c is MeshCollider mc && components[i].lastRaycastHit.triangleIndex != -1) {
+					var sharedMesh = mc.sharedMesh;
+					if (meshToIndex.TryGetValue(sharedMesh, out var meshIndex)) {
+						componentsByMesh[meshIndex].Add(components[i]);
+					} else if (sharedMesh != null && sharedMesh.isReadable) {
+						meshToIndex[sharedMesh] = meshes.Count;
+						meshes.Add(sharedMesh);
+						componentsByMesh.Add(ListPool<AIPathAlignedToSurface>.Claim());
+						componentsByMesh[meshes.Count-1].Add(components[i]);
+					} else {
+						// Unreadable mesh
+						components[i].SetInterpolatedNormal(components[i].lastRaycastHit.normal);
+					}
+				} else {
+					// Not a mesh collider, or the triangle index was -1
+					components[i].SetInterpolatedNormal(components[i].lastRaycastHit.normal);
+				}
+			}
+			var meshDatas = Mesh.AcquireReadOnlyMeshData(meshes);
+			for (int i = 0; i < meshes.Count; i++) {
+				var m = meshes[i];
+				var meshIndex = meshToIndex[m];
+				var meshData = meshDatas[meshIndex];
+				var componentsForMesh = componentsByMesh[meshIndex];
+
+				var stream = meshData.GetVertexAttributeStream(UnityEngine.Rendering.VertexAttribute.Normal);
+
+				if (stream == -1) {
+					// Mesh does not have normals
+					for (int j = 0; j < componentsForMesh.Count; j++) componentsForMesh[j].SetInterpolatedNormal(componentsForMesh[j].lastRaycastHit.normal);
+					continue;
+				}
+				var vertexData = meshData.GetVertexData<byte>(stream);
+				var stride = meshData.GetVertexBufferStride(stream);
+				var normalOffset = meshData.GetVertexAttributeOffset(UnityEngine.Rendering.VertexAttribute.Normal);
+				unsafe {
+					var normals = (byte*)vertexData.GetUnsafeReadOnlyPtr() + normalOffset;
+
+					for (int j = 0; j < componentsForMesh.Count; j++) {
+						var comp = componentsForMesh[j];
+						var hit = comp.lastRaycastHit;
+						int t0, t1, t2;
+
+						// Get the vertex indices corresponding to the triangle that was hit
+						if (meshData.indexFormat == UnityEngine.Rendering.IndexFormat.UInt16) {
+							var indices = meshData.GetIndexData<ushort>();
+							t0 = indices[hit.triangleIndex * 3 + 0];
+							t1 = indices[hit.triangleIndex * 3 + 1];
+							t2 = indices[hit.triangleIndex * 3 + 2];
+						} else {
+							var indices = meshData.GetIndexData<int>();
+							t0 = indices[hit.triangleIndex * 3 + 0];
+							t1 = indices[hit.triangleIndex * 3 + 1];
+							t2 = indices[hit.triangleIndex * 3 + 2];
+						}
+
+						// Get the normals corresponding to the 3 vertices
+						var n0 = *((Vector3*)(normals + t0 * stride));
+						var n1 = *((Vector3*)(normals + t1 * stride));
+						var n2 = *((Vector3*)(normals + t2 * stride));
+
+						// Interpolate the normal using the barycentric coordinates
+						Vector3 baryCenter = hit.barycentricCoordinate;
+						Vector3 interpolatedNormal = n0 * baryCenter.x + n1 * baryCenter.y + n2 * baryCenter.z;
+						interpolatedNormal = interpolatedNormal.normalized;
+						Transform hitTransform = hit.collider.transform;
+						interpolatedNormal = hitTransform.TransformDirection(interpolatedNormal);
+						comp.SetInterpolatedNormal(interpolatedNormal);
+					}
+				}
+			}
+			meshDatas.Dispose();
+			for (int i = 0; i < componentsByMesh.Count; i++) ListPool<AIPathAlignedToSurface>.Release(componentsByMesh[i]);
+			ListPool<Mesh>.Release(ref meshes);
+			scratchDictionary.Clear();
+			Profiler.EndSample();
+		}
+
+		void SetInterpolatedNormal (Vector3 normal) {
+			if (normal != Vector3.zero) {
+				var fwd = Vector3.Cross(movementPlane.rotation * Vector3.right, normal);
+				movementPlane = new Util.SimpleMovementPlane(Quaternion.LookRotation(fwd, normal));
+			}
+			if (rvoController != null) rvoController.movementPlane = movementPlane;
+		}
+
+		protected override void UpdateMovementPlane () {
+			// The UpdateMovementPlanes method will take care of this
+		}
+	}
+}