248 lines
9.4 KiB
C#
248 lines
9.4 KiB
C#
#pragma warning disable CS0282
|
|
#if MODULE_ENTITIES
|
|
using Unity.Entities;
|
|
using Unity.Mathematics;
|
|
using Unity.Burst;
|
|
using Unity.Collections;
|
|
|
|
namespace Pathfinding.ECS {
|
|
using System.Collections.Generic;
|
|
using System.Runtime.InteropServices;
|
|
using Pathfinding;
|
|
using Pathfinding.Util;
|
|
using Pathfinding.Collections;
|
|
using Unity.Transforms;
|
|
using UnityEngine.Profiling;
|
|
|
|
[UpdateBefore(typeof(RepairPathSystem))]
|
|
[UpdateInGroup(typeof(AIMovementSystemGroup))]
|
|
[RequireMatchingQueriesForUpdate]
|
|
[BurstCompile]
|
|
public partial struct MovementPlaneFromGraphSystem : ISystem {
|
|
public EntityQuery entityQueryGraph;
|
|
public EntityQuery entityQueryNormal;
|
|
// Store the queue in a GCHandle to avoid restrictions on ISystem
|
|
GCHandle graphNodeQueue;
|
|
|
|
public void OnCreate (ref SystemState state) {
|
|
entityQueryGraph = state.GetEntityQuery(ComponentType.ReadOnly<MovementState>(), ComponentType.ReadWrite<AgentMovementPlane>(), ComponentType.ReadOnly<AgentMovementPlaneSource>());
|
|
entityQueryGraph.SetSharedComponentFilter(new AgentMovementPlaneSource { value = MovementPlaneSource.Graph });
|
|
entityQueryNormal = state.GetEntityQuery(
|
|
ComponentType.ReadWrite<ManagedState>(),
|
|
ComponentType.ReadOnly<LocalTransform>(),
|
|
ComponentType.ReadWrite<AgentMovementPlane>(),
|
|
ComponentType.ReadOnly<AgentCylinderShape>(),
|
|
ComponentType.ReadOnly<AgentMovementPlaneSource>()
|
|
);
|
|
entityQueryNormal.AddSharedComponentFilter(new AgentMovementPlaneSource { value = MovementPlaneSource.NavmeshNormal });
|
|
|
|
graphNodeQueue = GCHandle.Alloc(new List<GraphNode>(32));
|
|
}
|
|
|
|
public void OnDestroy (ref SystemState state) {
|
|
graphNodeQueue.Free();
|
|
}
|
|
|
|
public void OnUpdate (ref SystemState systemState) {
|
|
var graphs = AstarPath.active?.data.graphs;
|
|
if (graphs == null) return;
|
|
|
|
var movementPlanes = CollectionHelper.CreateNativeArray<AgentMovementPlane>(graphs.Length, systemState.WorldUpdateAllocator, NativeArrayOptions.UninitializedMemory);
|
|
for (int i = 0; i < graphs.Length; i++) {
|
|
movementPlanes[i] = new AgentMovementPlane(MovementPlaneFromGraph(graphs[i]));
|
|
}
|
|
|
|
if (!entityQueryNormal.IsEmpty) {
|
|
Profiler.BeginSample("MovementPlaneSource.NavmeshNormal");
|
|
systemState.CompleteDependency();
|
|
var vertices = new NativeList<Int3>(16, Allocator.Temp);
|
|
new JobMovementPlaneFromNavmeshNormal {
|
|
dt = AIMovementSystemGroup.TimeScaledRateManager.CheapStepDeltaTime,
|
|
vertices = vertices,
|
|
que = (List<GraphNode>)graphNodeQueue.Target,
|
|
}.Run(entityQueryNormal);
|
|
Profiler.EndSample();
|
|
}
|
|
|
|
systemState.Dependency = new JobMovementPlaneFromGraph {
|
|
movementPlanes = movementPlanes,
|
|
}.Schedule(entityQueryGraph, systemState.Dependency);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Natural movement plane for a graph traversing a given graph.
|
|
///
|
|
/// This is the movement plane used for <see cref="MovementPlaneSource"/>.Graph.
|
|
///
|
|
/// See: <see cref="FollowerEntity.movementPlaneSource"/>
|
|
/// </summary>
|
|
public static NativeMovementPlane MovementPlaneFromGraph (NavGraph graph) {
|
|
if (graph is NavmeshBase navmesh) {
|
|
return new NativeMovementPlane(navmesh.transform.rotation);
|
|
} else if (graph is GridGraph grid) {
|
|
return new NativeMovementPlane(grid.transform.rotation);
|
|
} else {
|
|
return new NativeMovementPlane(quaternion.identity);
|
|
}
|
|
}
|
|
|
|
partial struct JobMovementPlaneFromNavmeshNormal : IJobEntity {
|
|
public float dt;
|
|
public NativeList<Int3> vertices;
|
|
public List<GraphNode> que;
|
|
|
|
public void Execute (ManagedState managedState, in LocalTransform localTransform, ref AgentMovementPlane agentMovementPlane, in AgentCylinderShape shape) {
|
|
var node = managedState.pathTracer.startNode as TriangleMeshNode;
|
|
if (node != null) {
|
|
// TODO: Expose this parameter?
|
|
const float InverseSmoothness = 20f;
|
|
var radius = math.max(0.01f, shape.radius);
|
|
SampleSmoothNavmeshNormal(node, que, vertices, localTransform.Position, radius, ref agentMovementPlane, dt * InverseSmoothness);
|
|
}
|
|
}
|
|
}
|
|
|
|
[BurstCompile]
|
|
partial struct JobMovementPlaneFromGraph : IJobEntity {
|
|
[ReadOnly]
|
|
public NativeArray<AgentMovementPlane> movementPlanes;
|
|
|
|
public void Execute (in MovementState movementState, ref AgentMovementPlane movementPlane) {
|
|
if (movementState.graphIndex < (uint)movementPlanes.Length) {
|
|
movementPlane = movementPlanes[(int)movementState.graphIndex];
|
|
} else {
|
|
// This can happen if the agent has no path, or if the path is stale.
|
|
// Potentially also if a graph has been removed.
|
|
}
|
|
}
|
|
}
|
|
|
|
public static void SampleSmoothNavmeshNormal (TriangleMeshNode node, List<GraphNode> scratchList, NativeList<Int3> scratchBuffer, float3 position, float agentRadius, ref AgentMovementPlane agentMovementPlane, float alpha) {
|
|
var vertices = scratchBuffer;
|
|
var que = scratchList;
|
|
vertices.Clear();
|
|
que.Clear();
|
|
int queStart = 0;
|
|
node.TemporaryFlag1 = true;
|
|
que.Add(node);
|
|
var i0 = node.v0;
|
|
var i1 = node.v1;
|
|
var i2 = node.v2;
|
|
|
|
while (queStart < que.Count) {
|
|
var current = que[queStart++] as TriangleMeshNode;
|
|
if (current == null) continue;
|
|
var anyVertex = current.v0 == i0 | current.v1 == i0 | current.v2 == i0 | current.v0 == i1 | current.v1 == i1 | current.v2 == i1 | current.v0 == i2 | current.v1 == i2 | current.v2 == i2;
|
|
if (anyVertex) {
|
|
current.GetVertices(out var v0, out var v1, out var v2);
|
|
vertices.Add(v0);
|
|
vertices.Add(v1);
|
|
vertices.Add(v2);
|
|
current.GetConnections((GraphNode con, ref List<GraphNode> que) => {
|
|
if (!con.TemporaryFlag1) {
|
|
con.TemporaryFlag1 = true;
|
|
que.Add(con);
|
|
}
|
|
}, ref que);
|
|
}
|
|
}
|
|
|
|
// Reset temporary flags
|
|
for (int i = 0; i < que.Count; i++) {
|
|
que[i].TemporaryFlag1 = false;
|
|
}
|
|
|
|
var verticesSpan = vertices.AsUnsafeSpan();
|
|
SampleSmoothTriangleNormal(ref position, ref verticesSpan, ref agentMovementPlane, agentRadius, alpha);
|
|
}
|
|
|
|
static float Square (float x) {
|
|
return x * x;
|
|
}
|
|
|
|
/// <summary>Sine of the angle ABC</summary>
|
|
static float SinAngle (float3 a, float3 b, float3 c) {
|
|
return math.sqrt(1 - Square(math.dot(math.normalizesafe(a - b), math.normalizesafe(c - b))));
|
|
}
|
|
|
|
[BurstCompile(FloatMode = FloatMode.Fast)]
|
|
static void SampleSmoothTriangleNormal (ref float3 position, ref UnsafeSpan<Int3> _triangleVertices, ref AgentMovementPlane agentMovementPlane, float agentRadius, float alpha) {
|
|
var triangleVertices = _triangleVertices.Reinterpret<int3>();
|
|
if (triangleVertices.Length < 3) throw new System.ArgumentException("triangleVertices.Length < 3");
|
|
unsafe {
|
|
// First 3 vertices represent the triangle we start on
|
|
var sourceVertices = triangleVertices.ptr;
|
|
var normals = stackalloc float3[3];
|
|
var weights = stackalloc float[3];
|
|
normals[0] = normals[1] = normals[2] = float3.zero;
|
|
weights[0] = weights[1] = weights[2] = 0;
|
|
var currentNormal = agentMovementPlane.value.up;
|
|
|
|
for (uint i = 0; i < triangleVertices.length; i += 3) {
|
|
var p0 = triangleVertices[i + 0];
|
|
var p1 = triangleVertices[i + 1];
|
|
var p2 = triangleVertices[i + 2];
|
|
var f0 = (float3)p0 * Int3.PrecisionFactor;
|
|
var f1 = (float3)p1 * Int3.PrecisionFactor;
|
|
var f2 = (float3)p2 * Int3.PrecisionFactor;
|
|
|
|
var triangleNormal = math.normalizesafe(math.cross(f1 - f0, f2 - f0));
|
|
|
|
const float COS_SMOOTH_ANGLE_LIMIT = 0.86f;
|
|
float weight = 1;
|
|
var cosAngle = math.dot(triangleNormal, currentNormal);
|
|
if (cosAngle < COS_SMOOTH_ANGLE_LIMIT) {
|
|
// Hard angle. Lower the weight of this triangle to avoid starting to rotate too early.
|
|
Polygon.ClosestPointOnTriangleByRef(in f0, in f1, in f2, in position, out var closest);
|
|
var distance = math.lengthsq(closest - position) / Square(1.5f * agentRadius);
|
|
var distanceWeight = math.max(0.1f, 1 - distance);
|
|
var angleWeight = (COS_SMOOTH_ANGLE_LIMIT - math.max(0, cosAngle)) / COS_SMOOTH_ANGLE_LIMIT;
|
|
weight = math.lerp(1, distanceWeight, angleWeight);
|
|
}
|
|
|
|
for (int j = 0; j < 3; j++) {
|
|
if (math.all(p0 == sourceVertices[j])) {
|
|
// When calculating smooth normals, we ideally want to weigh the contributions from
|
|
// differnt triangles by the angle of the triangle at the vertex.
|
|
// We use the sine of that angle instead, which is a decent approximation.
|
|
var w = weight * SinAngle(p2, p0, p1);
|
|
weights[j] += w;
|
|
normals[j] += w * triangleNormal;
|
|
}
|
|
}
|
|
|
|
for (int j = 0; j < 3; j++) {
|
|
if (math.all(p1 == sourceVertices[j])) {
|
|
var w = weight * SinAngle(p0, p1, p2);
|
|
weights[j] += w;
|
|
normals[j] += w * triangleNormal;
|
|
}
|
|
}
|
|
|
|
for (int j = 0; j < 3; j++) {
|
|
if (math.all(p2 == sourceVertices[j])) {
|
|
var w = weight * SinAngle(p1, p2, p0);
|
|
weights[j] += w;
|
|
normals[j] += w * triangleNormal;
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int j = 0; j < 3; j++) {
|
|
if (weights[j] > 0) normals[j] /= weights[j];
|
|
}
|
|
|
|
var v0 = (float3)sourceVertices[0] * Int3.PrecisionFactor;
|
|
var v1 = (float3)sourceVertices[1] * Int3.PrecisionFactor;
|
|
var v2 = (float3)sourceVertices[2] * Int3.PrecisionFactor;
|
|
var barycentric = Polygon.ClosestPointOnTriangleBarycentric(v0, v1, v2, position);
|
|
var targetNormal = math.normalizesafe(normals[0] * barycentric.x + normals[1] * barycentric.y + normals[2] * barycentric.z);
|
|
|
|
var nextNormal = math.lerp(currentNormal, targetNormal, math.clamp(alpha, 0, 1));
|
|
agentMovementPlane.value = agentMovementPlane.value.MatchUpDirection(nextNormal);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|