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
|
#if MODULE_ENTITIES
using System.Runtime.InteropServices;
using Pathfinding.Drawing;
using Pathfinding.PID;
using Pathfinding.Util;
using Unity.Burst;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Entities;
using Unity.Mathematics;
using Unity.Transforms;
namespace Pathfinding.ECS {
[BurstCompile]
struct DrawGizmosJobUtils {
[BurstCompile]
internal static void DrawPath (ref CommandBuilder draw, ref UnsafeSpan<float3> vertices, ref AgentCylinderShape shape) {
draw.PushColor(Palette.Colorbrewer.Set1.Orange);
// Some people will set the agent's radius to zero. In that case we just draw the path as a polyline as we have no good reference for how to space the symbols.
if (shape.radius > 0.01f) {
var generator = new CommandBuilder.PolylineWithSymbol(CommandBuilder.SymbolDecoration.ArrowHead, shape.radius * 0.5f, shape.radius * 0.0f, shape.radius * 4f, true);
for (int i = vertices.Length - 1; i >= 0; i--) generator.MoveTo(ref draw, vertices[i]);
} else {
for (int i = 0; i < vertices.Length - 1; i++) draw.Line(vertices[i], vertices[i+1]);
}
draw.PopColor();
}
}
public partial struct JobDrawFollowerGizmos : IJobChunk {
public CommandBuilder draw;
public GCHandle entityManagerHandle;
[ReadOnly]
public ComponentTypeHandle<LocalTransform> LocalTransformTypeHandleRO;
[ReadOnly]
public ComponentTypeHandle<AgentCylinderShape> AgentCylinderShapeHandleRO;
[ReadOnly]
public ComponentTypeHandle<MovementSettings> MovementSettingsHandleRO;
[ReadOnly]
public ComponentTypeHandle<AgentMovementPlane> AgentMovementPlaneHandleRO;
// This is actually not read only, because the GetNextCorners function can modify internal state
// See JobRepairPath.Scheduler.ManagedStateTypeHandleRW for details about why NativeDisableContainerSafetyRestriction is required
[NativeDisableContainerSafetyRestriction]
public ComponentTypeHandle<ManagedState> ManagedStateHandleRW;
[ReadOnly]
public ComponentTypeHandle<MovementState> MovementStateHandleRO;
[ReadOnly]
public ComponentTypeHandle<ResolvedMovement> ResolvedMovementHandleRO;
[NativeDisableContainerSafetyRestriction]
public NativeList<float3> scratchBuffer1;
[NativeDisableContainerSafetyRestriction]
public NativeArray<int> scratchBuffer2;
public void Execute (in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in Unity.Burst.Intrinsics.v128 chunkEnabledMask) {
if (!scratchBuffer1.IsCreated) scratchBuffer1 = new NativeList<float3>(32, Allocator.Temp);
if (!scratchBuffer2.IsCreated) scratchBuffer2 = new NativeArray<int>(32, Allocator.Temp);
unsafe {
var localTransforms = (LocalTransform*)chunk.GetNativeArray(ref LocalTransformTypeHandleRO).GetUnsafeReadOnlyPtr();
var agentCylinderShapes = (AgentCylinderShape*)chunk.GetNativeArray(ref AgentCylinderShapeHandleRO).GetUnsafeReadOnlyPtr();
var movementSettings = (MovementSettings*)chunk.GetNativeArray(ref MovementSettingsHandleRO).GetUnsafeReadOnlyPtr();
var movementPlanes = (AgentMovementPlane*)chunk.GetNativeArray(ref AgentMovementPlaneHandleRO).GetUnsafeReadOnlyPtr();
var managedStates = chunk.GetManagedComponentAccessor(ref ManagedStateHandleRW, (EntityManager)entityManagerHandle.Target);
var movementStates = (MovementState*)chunk.GetNativeArray(ref MovementStateHandleRO).GetUnsafeReadOnlyPtr();
var resolvedMovement = (ResolvedMovement*)chunk.GetNativeArray(ref ResolvedMovementHandleRO).GetUnsafeReadOnlyPtr();
for (int i = 0; i < chunk.Count; i++) {
Execute(ref localTransforms[i], ref movementPlanes[i], ref agentCylinderShapes[i], managedStates[i], ref movementSettings[i], ref movementStates[i], ref resolvedMovement[i]);
}
}
}
public void Execute (ref LocalTransform transform, ref AgentMovementPlane movementPlane, ref AgentCylinderShape shape, ManagedState managedState, ref MovementSettings settings, ref MovementState movementState, ref ResolvedMovement resolvedMovement) {
if ((settings.debugFlags & PIDMovement.DebugFlags.Funnel) != 0) {
managedState.pathTracer.DrawFunnel(draw, movementPlane.value);
}
if ((settings.debugFlags & PIDMovement.DebugFlags.Rotation) != 0) {
var p2D = movementPlane.value.ToPlane(transform.Position, out float positionElevation);
draw.PushMatrix(math.mul(new float4x4(movementPlane.value.rotation, float3.zero), float4x4.Translate(new float3(0, positionElevation, 0))));
var visualRotation = movementPlane.value.ToPlane(transform.Rotation);
var unsmoothedRotation = visualRotation - movementState.rotationOffset2;
var internalRotation = unsmoothedRotation - movementState.rotationOffset;
var targetInternalRotation = resolvedMovement.targetRotation;
var targetInternalRotationHint = resolvedMovement.targetRotationHint;
math.sincos(math.PI*0.5f + new float3(visualRotation, unsmoothedRotation, internalRotation), out var s, out var c);
draw.xz.ArrowheadArc(p2D, new float2(c.x, s.x), shape.radius * 1.1f, Palette.Colorbrewer.Set1.Blue);
draw.xz.ArrowheadArc(p2D, new float2(c.y, s.y), shape.radius * 1.1f, Palette.Colorbrewer.Set1.Purple);
draw.xz.ArrowheadArc(p2D, new float2(c.z, s.z), shape.radius * 1.1f, Palette.Colorbrewer.Set1.Orange);
math.sincos(math.PI*0.5f + new float2(targetInternalRotation, targetInternalRotationHint), out var s2, out var c2);
draw.xz.ArrowheadArc(p2D, new float2(c2.x, s2.x), shape.radius * 1.2f, Palette.Colorbrewer.Set1.Yellow);
draw.xz.ArrowheadArc(p2D, new float2(c2.y, s2.y), shape.radius * 1.2f, Palette.Colorbrewer.Set1.Pink);
draw.PopMatrix();
}
if ((settings.debugFlags & PIDMovement.DebugFlags.Path) != 0 && managedState.pathTracer.hasPath) {
scratchBuffer1.Clear();
managedState.pathTracer.GetNextCorners(scratchBuffer1, int.MaxValue, ref scratchBuffer2, Allocator.Temp, managedState.pathfindingSettings.traversalProvider, managedState.activePath);
var span = scratchBuffer1.AsUnsafeSpan();
DrawGizmosJobUtils.DrawPath(ref draw, ref span, ref shape);
}
}
}
}
#endif
|
