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
142
143
144
145
146
|
#if MODULE_ENTITIES
using Unity.Entities;
using Unity.Mathematics;
using Unity.Profiling;
using Unity.Transforms;
using Unity.Burst;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Pathfinding.Drawing;
using Pathfinding.PID;
using Unity.Burst.Intrinsics;
namespace Pathfinding.ECS {
[BurstCompile]
public partial struct JobControl : IJobEntity, IJobEntityChunkBeginEnd {
public float dt;
public CommandBuilder draw;
[ReadOnly]
[NativeDisableContainerSafetyRestriction]
public NavmeshEdges.NavmeshBorderData navmeshEdgeData;
[NativeDisableContainerSafetyRestriction]
public NativeList<float2> edgesScratch;
private static readonly ProfilerMarker MarkerConvertObstacles = new ProfilerMarker("ConvertObstacles");
public static float3 ClampToNavmesh (float3 position, float3 closestOnNavmesh, in AgentCylinderShape shape, in AgentMovementPlane movementPlane) {
// Don't clamp the elevation except to make sure it's not too far below the navmesh.
var clamped2D = movementPlane.value.ToPlane(closestOnNavmesh, out float clampedElevation);
movementPlane.value.ToPlane(position, out float currentElevation);
currentElevation = math.max(currentElevation, clampedElevation - shape.height * 0.4f);
position = movementPlane.value.ToWorld(clamped2D, currentElevation);
return position;
}
public bool OnChunkBegin (in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask) {
if (!edgesScratch.IsCreated) edgesScratch = new NativeList<float2>(64, Allocator.Temp);
return true;
}
public void OnChunkEnd (in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask, bool chunkWasExecuted) {}
public void Execute (ref LocalTransform transform, ref MovementState state, in DestinationPoint destination, in AgentCylinderShape shape, in AgentMovementPlane movementPlane, in MovementSettings settings, in ResolvedMovement resolvedMovement, ref MovementControl controlOutput) {
// Clamp the agent to the navmesh.
var position = ClampToNavmesh(transform.Position, state.closestOnNavmesh, in shape, in movementPlane);
edgesScratch.Clear();
var scale = math.abs(transform.Scale);
var settingsTemp = settings.follower;
// Scale the settings by the agent's scale
settingsTemp.ScaleByAgentScale(scale);
settingsTemp.desiredWallDistance *= resolvedMovement.turningRadiusMultiplier;
if (state.isOnValidNode) {
MarkerConvertObstacles.Begin();
var localBounds = PIDMovement.InterestingEdgeBounds(ref settingsTemp, position, state.nextCorner, shape.height, movementPlane.value);
navmeshEdgeData.GetEdgesInRange(state.hierarchicalNodeIndex, localBounds, edgesScratch, movementPlane.value);
MarkerConvertObstacles.End();
}
// To ensure we detect that the end of the path is reached robustly we make the agent move slightly closer.
// to the destination than the stopDistance.
const float FUZZ = 0.005f;
// If we are moving towards an off-mesh link, then we want the agent to stop precisely at the off-mesh link.
// TODO: Depending on the link, we may want the agent to move towards the link at full speed, instead of slowing down.
var stopDistance = state.traversingLastPart ? math.max(0, settings.stopDistance - FUZZ) : 0f;
var distanceToSteeringTarget = math.max(0, state.remainingDistanceToEndOfPart - stopDistance);
var rotation = movementPlane.value.ToPlane(transform.Rotation) - state.rotationOffset - state.rotationOffset2;
transform.Position = position;
if (dt > 0.000001f) {
if (!math.isfinite(distanceToSteeringTarget)) {
// The agent has no path, just stay still
controlOutput = new MovementControl {
targetPoint = position,
speed = 0,
endOfPath = position,
maxSpeed = settings.follower.speed,
overrideLocalAvoidance = false,
hierarchicalNodeIndex = state.hierarchicalNodeIndex,
targetRotation = resolvedMovement.targetRotation,
rotationSpeed = settings.follower.maxRotationSpeed,
targetRotationOffset = state.rotationOffset, // May be modified by other systems
};
} else if (settings.isStopped) {
// The user has requested that the agent slow down as quickly as possible.
// TODO: If the agent is not clamped to the navmesh, it should still move towards the navmesh if it is outside it.
controlOutput = new MovementControl {
// Keep moving in the same direction as during the last frame, but slow down
targetPoint = position + math.normalizesafe(resolvedMovement.targetPoint - position) * 10.0f,
speed = settings.follower.Accelerate(resolvedMovement.speed, settings.follower.slowdownTime, -dt),
endOfPath = state.endOfPath,
maxSpeed = settings.follower.speed,
overrideLocalAvoidance = false,
hierarchicalNodeIndex = state.hierarchicalNodeIndex,
targetRotation = resolvedMovement.targetRotation,
rotationSpeed = settings.follower.maxRotationSpeed,
targetRotationOffset = state.rotationOffset, // May be modified by other systems
};
} else {
var controlParams = new PIDMovement.ControlParams {
edges = edgesScratch.AsArray(),
nextCorner = state.nextCorner,
agentRadius = shape.radius,
facingDirectionAtEndOfPath = destination.facingDirection,
endOfPath = state.endOfPath,
remainingDistance = distanceToSteeringTarget,
closestOnNavmesh = state.closestOnNavmesh,
debugFlags = settings.debugFlags,
p = position,
rotation = rotation,
maxDesiredWallDistance = state.followerState.maxDesiredWallDistance,
speed = controlOutput.speed,
movementPlane = movementPlane.value,
};
var control = PIDMovement.Control(ref settingsTemp, dt, ref controlParams, ref draw, out state.followerState.maxDesiredWallDistance);
var positionDelta = movementPlane.value.ToWorld(control.positionDelta, 0);
var speed = math.length(positionDelta) / dt;
controlOutput = new MovementControl {
targetPoint = position + math.normalizesafe(positionDelta) * distanceToSteeringTarget,
speed = speed,
endOfPath = state.endOfPath,
maxSpeed = settingsTemp.speed * 1.1f,
overrideLocalAvoidance = false,
hierarchicalNodeIndex = state.hierarchicalNodeIndex,
// It may seem sketchy to use a target rotation so close to the current rotation. One might think
// there's risk of overshooting this target rotation if the frame rate is uneven.
// But the TimeScaledRateManager ensures that this is not the case.
// The cheap simulation's time (which is the one actually rotating the agent) is always guaranteed to be
// behind (or precisely caught up with) the full simulation's time (that's the simulation which runs this system).
targetRotation = rotation + control.rotationDelta,
targetRotationHint = rotation + AstarMath.DeltaAngle(rotation, control.targetRotation),
rotationSpeed = math.abs(control.rotationDelta / dt),
targetRotationOffset = state.rotationOffset, // May be modified by other systems
};
}
} else {
controlOutput.hierarchicalNodeIndex = -1;
}
}
}
}
#endif
|
