1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
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
}
}
}
|
