using System; using System.Collections.Generic; using UnityEngine; using Unity.Jobs; using Unity.Burst; using Unity.Mathematics; using Unity.Collections; using Pathfinding.RVO; namespace M7 { /// /// Adapted from A* Pathfinder project's LightweightRVO.cs example by FDefoy 2021-12-01 /// public class CollisionAvoidanceProvider : MonoBehaviour { [Header("My Settings")] [SerializeField] int jobSetAgentsBatchSize = 1; [SerializeField] int jobMoveAgentsBatchSize = 1; /// How far in the future too look for agents public float agentTimeHorizon = 10; // How far in the future too look for agents [HideInInspector] /// How far in the future too look for obstacles public float obstacleTimeHorizon = 10; // How far in the future too look for obstacles /// Max number of neighbour agents to take into account public int maxNeighbours = 10; // Max number of neighbour agents to take into account /// Enable the debug flag for all agents public bool debug = false; // Enable the debug flag for all agents /// Reference to the simulator in the scene internal SimulatorBurst burstSim; internal NativeArray interpolatedVelocities = default; internal NativeArray interpolatedRotations = default; internal int agentCount { get; private set; } //internal NativeArray radius { get { return burstSim.simulationData.radius; } } //internal NativeArray position { get { return burstSim.simulationData.position; } } internal Action OnInitialized; internal Action OnJobScheduled; internal Action OnJobCompleted; /// All agents handled by this script List agents; /// Goals for each agent NativeArray goals = default; internal JobHandle job; float agentRadius; float agentMaxSpeed; // Set the max speed agents can accelerate to to avoid a collision bool initialized; void OnDestroy () { if (goals != default) goals.Dispose(); if (interpolatedVelocities != default) interpolatedVelocities.Dispose(); if (interpolatedRotations != default) interpolatedRotations.Dispose(); } void Awake() { RVOSimulator rvoSim = RVOSimulator.active; if (rvoSim == null) { Debug.LogError("No RVOSimulator could be found in the scene. Please add a RVOSimulator component to any GameObject"); return; } burstSim = rvoSim.GetSimulator() as SimulatorBurst; if (burstSim == null) { throw new System.Exception("Burst must be active in the RVOSimulator"); } } public void Initialize (int poolsize, float agentRadius, float agentMaxSpeed) { // TODO: Change max speed to maxSpeedBoost? Otherwise all agents share this value Debug.Log("Initialize Collision avoidance provider"); agents = new List(poolsize); this.agentRadius = agentRadius; this.agentMaxSpeed = agentMaxSpeed; Pathfinding.Util.Memory.Realloc(ref goals, poolsize, Allocator.Persistent); // TODO: = NativeArray.RESIZE()? Pathfinding.Util.Memory.Realloc(ref interpolatedVelocities, poolsize, Allocator.Persistent, NativeArrayOptions.ClearMemory); Pathfinding.Util.Memory.Realloc(ref interpolatedRotations, poolsize, Allocator.Persistent, NativeArrayOptions.ClearMemory); burstSim.ClearAgents(); initialized = true; } internal void UpdateLoop() { ScheduleJob(); CompleteJob(); } private void ScheduleJob() { job = UpdatePositionsJob(); // Update positions using the simulation OnJobScheduled?.Invoke(); // Replaces GenerateMeshJob(); } private void CompleteJob() { job.Complete(); OnJobCompleted?.Invoke(); // Replaces meshJob.Complete(); } internal void AddAgent(int agentIndex, float3 currentPosition, float3 goal) { if (initialized == false) { Debug.LogError("CollisionAvoidanceProvider Must be initialized before adding agents"); } agents.Add( burstSim.AddAgent(currentPosition) ); goals[agentIndex] = goal; agentCount++; } internal void ActivateAgents() { SetupAgentsJob().Complete(); } JobHandle SetupAgentsJob() { // Create a job to init entity data return new JobSetupAgents { data = burstSim.simulationData, goals = goals, radius = agentRadius, agentTimeHorizon = agentTimeHorizon, obstacleTimeHorizon = obstacleTimeHorizon, maxNeighbours = maxNeighbours, debug = debug, maxSpeed = agentMaxSpeed, }.Schedule(agentCount, jobSetAgentsBatchSize); } JobHandle UpdatePositionsJob() { // Create a job to apply movement to entity data and update a few values return new JobMoveAgents { interpolatedVelocities = interpolatedVelocities, interpolatedRotations = interpolatedRotations, agentPositions = burstSim.simulationData.position, agentMaxSpeeds = burstSim.simulationData.maxSpeed, agentTargetPoints = burstSim.outputData.targetPoint, agentSpeeds = burstSim.outputData.speed, deltaTime = Time.deltaTime, debug = debug, }.Schedule(agentCount, jobMoveAgentsBatchSize); } [BurstCompile(CompileSynchronously = true)] struct JobSetupAgents : IJobParallelFor { public SimulatorBurst.AgentData data; [ReadOnly] public NativeArray goals; [ReadOnly] public float radius; [ReadOnly] public float agentTimeHorizon; [ReadOnly] public float obstacleTimeHorizon; [ReadOnly] public int maxNeighbours; [ReadOnly] public bool debug; [ReadOnly] public float maxSpeed; float dist; public void Execute(int index) { data.radius[index] = radius; data.agentTimeHorizon[index] = agentTimeHorizon; data.obstacleTimeHorizon[index] = obstacleTimeHorizon; data.maxNeighbours[index] = maxNeighbours; data.debugDraw[index] = index == 0 && debug; // Set the desired velocity for the agent dist = math.length(goals[index] - data.position[index]); data.SetTarget(index, goals[index], math.min(dist, maxSpeed), maxSpeed * 1.1f); // Decrease agent priority when it is close to its goal. // Also protect against division by zero. data.priority[index] = math.max(0.1f, 1 - math.exp(-0.5f * dist / math.max(maxSpeed, 0.1f))); } } [BurstCompile(CompileSynchronously = true)] struct JobMoveAgents : IJobParallelFor { public NativeArray interpolatedVelocities; public NativeArray interpolatedRotations; public NativeArray agentPositions; [ReadOnly] public NativeArray agentTargetPoints; [ReadOnly] public NativeArray agentMaxSpeeds; [ReadOnly] public NativeArray agentSpeeds; [ReadOnly] public float deltaTime; [ReadOnly] public bool debug; float3 pos; float3 deltaPosition; float length; float maxLength; float3 newPos; float speed; float maxSpeed; float vLength; float2 interpolatedVelocity; public void Execute (int index) { speed = agentSpeeds[index]; maxSpeed = agentMaxSpeeds[index]; pos = agentPositions[index]; deltaPosition = agentTargetPoints[index] - pos; length = math.length(deltaPosition); maxLength = speed * deltaTime; // Clamp the movement delta to a maximum length if (length > maxLength && length > 0) deltaPosition *= maxLength / length; newPos = pos + deltaPosition; if (!debug) { // If not in debug mode agentPositions[index] = newPos; // Set the agent's new position } interpolatedVelocity = interpolatedVelocities[index] += deltaPosition.xz; vLength = math.length(interpolatedVelocity); if (vLength > maxSpeed * 0.1f && vLength > 0) { interpolatedVelocity *= maxSpeed * 0.1f / vLength; interpolatedRotations[index] = math.lerp(interpolatedRotations[index], interpolatedVelocity, math.saturate(speed * deltaTime*4f)); } } } } }