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
|
#if MODULE_ENTITIES
using Unity.Burst;
using Unity.Entities;
using Unity.Mathematics;
using Unity.Transforms;
namespace Pathfinding.ECS {
[BurstCompile]
public partial struct JobMoveAgent : IJobEntity {
public float dt;
static void UpdateVelocityEstimate (ref LocalTransform transform, ref MovementStatistics movementStatistics, float dt) {
if (dt > 0.000001f) {
movementStatistics.estimatedVelocity = (transform.Position - movementStatistics.lastPosition) / dt;
}
}
static void ResolveRotation (ref LocalTransform transform, ref MovementState state, in ResolvedMovement resolvedMovement, in MovementSettings movementSettings, in AgentMovementPlane movementPlane, float dt) {
var currentRotation = movementPlane.value.ToPlane(transform.Rotation);
var currentInternalRotation = currentRotation - state.rotationOffset - state.rotationOffset2;
var deltaRotation = math.clamp(AstarMath.DeltaAngle(currentInternalRotation, resolvedMovement.targetRotation), -resolvedMovement.rotationSpeed * dt, resolvedMovement.rotationSpeed * dt);
var extraRotationSpeed = math.radians(movementSettings.follower.maxRotationSpeed) * 0.5f;
var deltaExtraRotation = math.clamp(AstarMath.DeltaAngle(state.rotationOffset, resolvedMovement.targetRotationOffset), -extraRotationSpeed * dt, extraRotationSpeed * dt);
var currentUnsmoothedRotation = currentInternalRotation + state.rotationOffset;
var newInternalRotation = currentInternalRotation + deltaRotation;
// Keep track of how much extra rotation we are applying. This is done so that
// the movement calculation can separate this out when doing its movement calculations.
state.rotationOffset += deltaExtraRotation;
// Make sure the rotation offset is between -pi/2 and pi/2 radians
state.rotationOffset = AstarMath.DeltaAngle(0, state.rotationOffset);
var newUnsmoothedRotation = newInternalRotation + state.rotationOffset;
if (movementSettings.rotationSmoothing > 0) {
// Apply compensation to rotationOffset2 to precisely cancel out the agent's rotation during this frame
state.rotationOffset2 += currentUnsmoothedRotation - newUnsmoothedRotation;
state.rotationOffset2 = AstarMath.DeltaAngle(0, state.rotationOffset2);
// Decay the rotationOffset2. This implicitly adds an exponential moving average to the visual rotation
var decay = math.abs(AstarMath.DeltaAngle(currentRotation, resolvedMovement.targetRotationHint)) / movementSettings.rotationSmoothing;
var exponentialDecay = decay*dt;
// In addition to an exponential decay, we also add a linear decay.
// This is important to relatively quickly zero out the error when the agent is almost
// facing the right direction. With an exponential decay, it would take far too long to look good.
const float LINEAR_DECAY_AMOUNT = 0.1f;
var linearDecay = (LINEAR_DECAY_AMOUNT/movementSettings.rotationSmoothing)*dt;
if (math.abs(state.rotationOffset2) > 0) state.rotationOffset2 *= math.max(0, 1 - exponentialDecay - linearDecay/math.abs(state.rotationOffset2));
} else if (state.rotationOffset2 != 0) {
// Rotation smoothing is disabled, decay the rotation offset very quickly, but still avoid jarring changes
state.rotationOffset2 += math.clamp(-state.rotationOffset2, -extraRotationSpeed * dt, extraRotationSpeed * dt);
}
transform.Rotation = movementPlane.value.ToWorldRotation(newInternalRotation + state.rotationOffset + state.rotationOffset2);
}
public static float3 MoveWithoutGravity (ref LocalTransform transform, in ResolvedMovement resolvedMovement, in AgentMovementPlane movementPlane, float dt) {
UnityEngine.Assertions.Assert.IsTrue(math.all(math.isfinite(resolvedMovement.targetPoint)));
// Move only along the movement plane
var localDir = movementPlane.value.ToPlane(resolvedMovement.targetPoint - transform.Position);
var magn = math.length(localDir);
var localDelta = math.select(localDir, localDir * math.clamp(resolvedMovement.speed * dt / magn, 0, 1.0f), magn > 0.0001f);
var delta = movementPlane.value.ToWorld(localDelta, 0);
return delta;
}
public static void ResolvePositionSmoothing (float3 movementDelta, ref MovementState state, in MovementSettings movementSettings, float dt) {
if (movementSettings.positionSmoothing > 0) {
state.positionOffset -= movementDelta;
var exponentialDecay = 1f/movementSettings.positionSmoothing*dt;
var linearDecay = 0.1f/movementSettings.positionSmoothing*dt;
var positionOffsetMagnitude = math.length(state.positionOffset);
if (positionOffsetMagnitude > 0) state.positionOffset *= math.max(0, 1 - exponentialDecay - linearDecay/positionOffsetMagnitude);
} else {
state.positionOffset = float3.zero;
}
}
public void Execute (ref LocalTransform transform, in AgentCylinderShape shape, in AgentMovementPlane movementPlane, ref MovementState state, in MovementSettings movementSettings, in ResolvedMovement resolvedMovement, ref MovementStatistics movementStatistics) {
MoveAgent(ref transform, in shape, in movementPlane, ref state, in movementSettings, in resolvedMovement, ref movementStatistics, dt);
}
public static void MoveAgent (ref LocalTransform transform, in AgentCylinderShape shape, in AgentMovementPlane movementPlane, ref MovementState state, in MovementSettings movementSettings, in ResolvedMovement resolvedMovement, ref MovementStatistics movementStatistics, float dt) {
var delta = MoveWithoutGravity(ref transform, in resolvedMovement, in movementPlane, dt);
UnityEngine.Assertions.Assert.IsTrue(math.all(math.isfinite(delta)), "Refusing to set the agent's position to a non-finite vector");
transform.Position += delta;
ResolvePositionSmoothing(delta, ref state, in movementSettings, dt);
ResolveRotation(ref transform, ref state, in resolvedMovement, in movementSettings, in movementPlane, dt);
UpdateVelocityEstimate(ref transform, ref movementStatistics, dt);
movementStatistics.lastPosition = transform.Position;
}
}
}
#endif
|
