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));
}
}
}
}
}