305 lines
8.0 KiB
C#
305 lines
8.0 KiB
C#
|
using System;
|
||
|
|
using HG;
|
||
|
|
using UnityEngine;
|
||
|
|
|
||
|
|
namespace RoR2.Navigation;
|
||
|
|
|
||
|
|
public class NodeGraphNavigationSystem : BroadNavigationSystem
|
||
|
|
{
|
||
|
|
public struct AgentData
|
||
|
|
{
|
||
|
|
public NodeGraph nodeGraph;
|
||
|
|
|
||
|
|
public PathFollower pathFollower;
|
||
|
|
|
||
|
|
public NodeGraph.PathRequest pathRequest;
|
||
|
|
|
||
|
|
public HullClassification hullClassification;
|
||
|
|
|
||
|
|
public PathTask pathTask;
|
||
|
|
|
||
|
|
public float updateInterval;
|
||
|
|
|
||
|
|
public float nextUpdate;
|
||
|
|
|
||
|
|
public bool targetReachable;
|
||
|
|
|
||
|
|
public float lastTargetReachableUpdate;
|
||
|
|
|
||
|
|
public NodeGraph.NodeIndex? lastKnownNode;
|
||
|
|
|
||
|
|
public float timeLastKnownNodeEncountered;
|
||
|
|
|
||
|
|
public float estimatedArrivalTimeAtLastKnownNode;
|
||
|
|
|
||
|
|
private PathFollower.Drawer pathDrawer;
|
||
|
|
|
||
|
|
public bool drawPath
|
||
|
|
{
|
||
|
|
get
|
||
|
|
{
|
||
|
|
return pathDrawer != null;
|
||
|
|
}
|
||
|
|
set
|
||
|
|
{
|
||
|
|
if (drawPath != value)
|
||
|
|
{
|
||
|
|
if (!value)
|
||
|
|
{
|
||
|
|
pathDrawer.Dispose();
|
||
|
|
pathDrawer = null;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
pathDrawer = new PathFollower.Drawer(pathFollower);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public void Initialize()
|
||
|
|
{
|
||
|
|
nextUpdate = float.NegativeInfinity;
|
||
|
|
nodeGraph = null;
|
||
|
|
updateInterval = 2f;
|
||
|
|
hullClassification = HullClassification.Human;
|
||
|
|
pathFollower = pathFollower ?? new PathFollower();
|
||
|
|
pathRequest = pathRequest ?? new NodeGraph.PathRequest();
|
||
|
|
targetReachable = false;
|
||
|
|
lastTargetReachableUpdate = float.NegativeInfinity;
|
||
|
|
lastKnownNode = null;
|
||
|
|
timeLastKnownNodeEncountered = float.NegativeInfinity;
|
||
|
|
estimatedArrivalTimeAtLastKnownNode = float.NegativeInfinity;
|
||
|
|
drawPath = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
public void Dispose()
|
||
|
|
{
|
||
|
|
drawPath = false;
|
||
|
|
nodeGraph = null;
|
||
|
|
nextUpdate = float.NegativeInfinity;
|
||
|
|
pathFollower.Reset();
|
||
|
|
pathRequest.Reset();
|
||
|
|
pathTask = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
public void InvalidatePath()
|
||
|
|
{
|
||
|
|
nextUpdate = float.NegativeInfinity;
|
||
|
|
pathTask = null;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public new struct Agent
|
||
|
|
{
|
||
|
|
private readonly BroadNavigationSystem.Agent inner;
|
||
|
|
|
||
|
|
private NodeGraphNavigationSystem system => (NodeGraphNavigationSystem)inner.system;
|
||
|
|
|
||
|
|
private ref AgentData agentData => ref system.allAgentData[(int)inner.handle];
|
||
|
|
|
||
|
|
public NodeGraph nodeGraph
|
||
|
|
{
|
||
|
|
get
|
||
|
|
{
|
||
|
|
return agentData.nodeGraph;
|
||
|
|
}
|
||
|
|
set
|
||
|
|
{
|
||
|
|
agentData.nodeGraph = value;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public HullClassification hullClassification
|
||
|
|
{
|
||
|
|
get
|
||
|
|
{
|
||
|
|
return agentData.hullClassification;
|
||
|
|
}
|
||
|
|
set
|
||
|
|
{
|
||
|
|
agentData.hullClassification = value;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public bool drawPath
|
||
|
|
{
|
||
|
|
get
|
||
|
|
{
|
||
|
|
return agentData.drawPath;
|
||
|
|
}
|
||
|
|
set
|
||
|
|
{
|
||
|
|
agentData.drawPath = value;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public PathFollower pathFollower
|
||
|
|
{
|
||
|
|
get
|
||
|
|
{
|
||
|
|
return agentData.pathFollower;
|
||
|
|
}
|
||
|
|
set
|
||
|
|
{
|
||
|
|
agentData.pathFollower = value;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public Agent(BroadNavigationSystem.Agent inner)
|
||
|
|
{
|
||
|
|
this.inner = inner;
|
||
|
|
}
|
||
|
|
|
||
|
|
public void InvalidatePath()
|
||
|
|
{
|
||
|
|
agentData.InvalidatePath();
|
||
|
|
}
|
||
|
|
|
||
|
|
public void DebugDrawPath(Color color, float duration)
|
||
|
|
{
|
||
|
|
agentData.pathFollower.DebugDrawPath(color, duration);
|
||
|
|
}
|
||
|
|
|
||
|
|
public static explicit operator Agent(BroadNavigationSystem.Agent other)
|
||
|
|
{
|
||
|
|
return new Agent(other);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private AgentData[] allAgentData = Array.Empty<AgentData>();
|
||
|
|
|
||
|
|
private int maxAgentUpdatesPerFrame = 1;
|
||
|
|
|
||
|
|
private int remainingAgentUpdateBudget;
|
||
|
|
|
||
|
|
public new Agent GetAgent(AgentHandle agentHandle)
|
||
|
|
{
|
||
|
|
return new Agent(base.GetAgent(agentHandle));
|
||
|
|
}
|
||
|
|
|
||
|
|
protected override void CreateAgent(in AgentHandle index)
|
||
|
|
{
|
||
|
|
ArrayUtils.EnsureCapacity(ref allAgentData, base.agentCount);
|
||
|
|
allAgentData[(int)index].Initialize();
|
||
|
|
}
|
||
|
|
|
||
|
|
protected override void DestroyAgent(in AgentHandle index)
|
||
|
|
{
|
||
|
|
allAgentData[(int)index].Dispose();
|
||
|
|
}
|
||
|
|
|
||
|
|
protected override void UpdateAgents(float deltaTime)
|
||
|
|
{
|
||
|
|
remainingAgentUpdateBudget = maxAgentUpdatesPerFrame;
|
||
|
|
for (AgentHandle agentHandle = (AgentHandle)0; (int)agentHandle < allAgentData.Length; agentHandle++)
|
||
|
|
{
|
||
|
|
UpdateAgent(in agentHandle);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
protected override void ConfigureAgentFromBody(in AgentHandle index, CharacterBody body)
|
||
|
|
{
|
||
|
|
ref AgentData reference = ref allAgentData[(int)index];
|
||
|
|
if ((bool)body)
|
||
|
|
{
|
||
|
|
reference.hullClassification = body.hullClassification;
|
||
|
|
reference.nodeGraph = (SceneInfo.instance ? SceneInfo.instance.GetNodeGraph(body.isFlying ? MapNodeGroup.GraphType.Air : MapNodeGroup.GraphType.Ground) : null);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
reference.hullClassification = HullClassification.Human;
|
||
|
|
reference.nodeGraph = null;
|
||
|
|
reference.InvalidatePath();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
protected override void InvalidateAgentPath(in AgentHandle index)
|
||
|
|
{
|
||
|
|
allAgentData[(int)index].InvalidatePath();
|
||
|
|
}
|
||
|
|
|
||
|
|
private void UpdateAgent(in AgentHandle agentHandle)
|
||
|
|
{
|
||
|
|
bool flag = false;
|
||
|
|
ref readonly BaseAgentData agentData = ref GetAgentData(in agentHandle);
|
||
|
|
if (!agentData.enabled)
|
||
|
|
{
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
ref AgentData reference = ref allAgentData[(int)agentHandle];
|
||
|
|
if (reference.pathTask == null && reference.nextUpdate <= localTime && remainingAgentUpdateBudget != 0)
|
||
|
|
{
|
||
|
|
flag = true;
|
||
|
|
remainingAgentUpdateBudget--;
|
||
|
|
reference.nextUpdate = localTime + reference.updateInterval;
|
||
|
|
if (reference.lastKnownNode != reference.pathFollower.nextNode)
|
||
|
|
{
|
||
|
|
reference.lastKnownNode = reference.pathFollower.nextNode;
|
||
|
|
reference.timeLastKnownNodeEncountered = localTime;
|
||
|
|
Vector3? nextPosition = reference.pathFollower.GetNextPosition();
|
||
|
|
if (nextPosition.HasValue && agentData.currentPosition.HasValue && agentData.maxWalkSpeed > 0f)
|
||
|
|
{
|
||
|
|
reference.estimatedArrivalTimeAtLastKnownNode = localTime + (nextPosition.Value - agentData.currentPosition.Value).magnitude / agentData.maxWalkSpeed;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
reference.estimatedArrivalTimeAtLastKnownNode = float.PositiveInfinity;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (agentData.currentPosition.HasValue && agentData.goalPosition.HasValue && (bool)reference.nodeGraph && reference.lastKnownNode.HasValue)
|
||
|
|
{
|
||
|
|
reference.pathRequest.startPos = agentData.currentPosition.Value;
|
||
|
|
if (localTime <= reference.estimatedArrivalTimeAtLastKnownNode + 1f && reference.lastKnownNode.Value.nodeIndex != -1)
|
||
|
|
{
|
||
|
|
reference.nodeGraph.GetNodePosition(reference.lastKnownNode.Value, out var position);
|
||
|
|
float num = (agentData.maxWalkSpeed + reference.updateInterval) * 1.5f;
|
||
|
|
float num2 = num * num;
|
||
|
|
if ((position - agentData.currentPosition.Value).sqrMagnitude < num2 && !Physics.Linecast(agentData.currentPosition.Value, position + new Vector3(0f, 0.2f, 0f), out var _, LayerIndex.world.intVal, QueryTriggerInteraction.Ignore))
|
||
|
|
{
|
||
|
|
reference.pathRequest.startPos = reference.lastKnownNode.Value;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
reference.pathRequest.endPos = agentData.goalPosition.Value;
|
||
|
|
reference.pathRequest.hullClassification = reference.hullClassification;
|
||
|
|
reference.pathRequest.maxJumpHeight = agentData.maxJumpHeight;
|
||
|
|
reference.pathRequest.maxSlope = agentData.maxSlopeAngle;
|
||
|
|
reference.pathRequest.maxSpeed = agentData.maxWalkSpeed;
|
||
|
|
reference.pathRequest.path = new Path(reference.nodeGraph);
|
||
|
|
reference.pathTask = reference.nodeGraph.ComputePath(reference.pathRequest);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (reference.pathTask != null && (flag || remainingAgentUpdateBudget != 0))
|
||
|
|
{
|
||
|
|
if (!flag)
|
||
|
|
{
|
||
|
|
remainingAgentUpdateBudget--;
|
||
|
|
flag = true;
|
||
|
|
}
|
||
|
|
if (reference.pathTask.status == PathTask.TaskStatus.Complete)
|
||
|
|
{
|
||
|
|
reference.pathFollower.SetPath(reference.pathTask.path);
|
||
|
|
reference.lastTargetReachableUpdate = localTime;
|
||
|
|
reference.targetReachable = reference.pathTask.wasReachable;
|
||
|
|
reference.pathTask = null;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (agentData.currentPosition.HasValue)
|
||
|
|
{
|
||
|
|
reference.pathFollower.UpdatePosition(agentData.currentPosition.Value);
|
||
|
|
}
|
||
|
|
Vector3? nextPosition2 = reference.pathFollower.GetNextPosition();
|
||
|
|
if (reference.targetReachable && !nextPosition2.HasValue)
|
||
|
|
{
|
||
|
|
nextPosition2 = agentData.goalPosition;
|
||
|
|
}
|
||
|
|
AgentOutput agentOutput = default(AgentOutput);
|
||
|
|
agentOutput.nextPosition = nextPosition2;
|
||
|
|
agentOutput.desiredJumpVelocity = (reference.pathFollower.nextWaypointNeedsJump ? reference.pathFollower.CalculateJumpVelocityNeededToReachNextWaypoint(agentData.maxWalkSpeed) : 0f);
|
||
|
|
agentOutput.targetReachable = reference.targetReachable;
|
||
|
|
agentOutput.lastPathUpdate = reference.lastTargetReachableUpdate;
|
||
|
|
AgentOutput output = agentOutput;
|
||
|
|
SetAgentOutput(in agentHandle, in output);
|
||
|
|
}
|
||
|
|
}
|