protected override void OnUpdate()
{
var commandBuffer = barrier.CreateCommandBuffer().AsParallelWriter();
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
var translationFromEntity = GetComponentDataFromEntity <Translation>(true);
var jumpingFromEntity = GetComponentDataFromEntity <NavJumping>(true);
var pathBufferFromEntity = GetBufferFromEntity <NavPathBufferElement>();
var jumpBufferFromEntity = GetBufferFromEntity <NavJumpBufferElement>();
var navMeshQueryPointerArray = World.GetExistingSystem <NavMeshQuerySystem>().PointerArray;
Entities
.WithNone <NavHasProblem>()
.WithAll <NavPlanning, LocalToParent>()
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(jumpingFromEntity)
.WithNativeDisableParallelForRestriction(pathBufferFromEntity)
.WithNativeDisableParallelForRestriction(jumpBufferFromEntity)
.WithNativeDisableParallelForRestriction(navMeshQueryPointerArray)
.ForEach((Entity entity, int entityInQueryIndex, int nativeThreadIndex, ref NavAgent agent, in Parent surface) =>
{
if (
surface.Value.Equals(Entity.Null) ||
agent.DestinationSurface.Equals(Entity.Null) ||
!localToWorldFromEntity.HasComponent(surface.Value) ||
!localToWorldFromEntity.HasComponent(agent.DestinationSurface)
)
{
return;
}
var agentPosition = localToWorldFromEntity[entity].Position;
var worldPosition = agentPosition;
var worldDestination = NavUtil.MultiplyPoint3x4(
localToWorldFromEntity[agent.DestinationSurface].Value,
agent.LocalDestination
);
var jumping = jumpingFromEntity.HasComponent(entity);
if (jumping)
{
worldPosition = worldDestination;
worldDestination = agentPosition;
}
var navMeshQueryPointer = navMeshQueryPointerArray[nativeThreadIndex];
UnsafeUtility.CopyPtrToStructure(navMeshQueryPointer.Value, out NavMeshQuery navMeshQuery);
var status = navMeshQuery.BeginFindPath(
navMeshQuery.MapLocation(worldPosition, Vector3.one * NavConstants.PATH_SEARCH_MAX, agent.TypeID),
navMeshQuery.MapLocation(worldDestination, Vector3.one * NavConstants.PATH_SEARCH_MAX, agent.TypeID),
NavMesh.AllAreas
);
while (NavUtil.HasStatus(status, PathQueryStatus.InProgress))
{
status = navMeshQuery.UpdateFindPath(
NavConstants.ITERATION_MAX,
out int iterationsPerformed
);
}
if (!NavUtil.HasStatus(status, PathQueryStatus.Success))
{
commandBuffer.RemoveComponent <NavPlanning>(entityInQueryIndex, entity);
commandBuffer.RemoveComponent <NavNeedsDestination>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavHasProblem>(entityInQueryIndex, entity, new NavHasProblem
{
Value = status
});
return;
}
navMeshQuery.EndFindPath(out int pathLength);
var polygonIdArray = new NativeArray <PolygonId>(
NavConstants.PATH_NODE_MAX,
Allocator.Temp
);
navMeshQuery.GetPathResult(polygonIdArray);
var len = pathLength + 1;
var straightPath = new NativeArray <NavMeshLocation>(len, Allocator.Temp);
var straightPathFlags = new NativeArray <StraightPathFlags>(len, Allocator.Temp);
var vertexSide = new NativeArray <float>(len, Allocator.Temp);
var straightPathCount = 0;
status = PathUtils.FindStraightPath(
navMeshQuery,
worldPosition,
worldDestination,
polygonIdArray,
pathLength,
ref straightPath,
ref straightPathFlags,
ref vertexSide,
ref straightPathCount,
NavConstants.PATH_NODE_MAX
);
var jumpBuffer = !jumpBufferFromEntity.HasComponent(entity) ? commandBuffer.AddBuffer <NavJumpBufferElement>(entityInQueryIndex, entity) : jumpBufferFromEntity[entity];
var pathBuffer = !pathBufferFromEntity.HasComponent(entity) ? commandBuffer.AddBuffer <NavPathBufferElement>(entityInQueryIndex, entity) : pathBufferFromEntity[entity];
if (jumping)
{
var lastValidPoint = float3.zero;
for (int i = 0; i < straightPath.Length; ++i)
{
if (navMeshQuery.IsValid(straightPath[i].polygon))
{
lastValidPoint = straightPath[i].position;
}
else
{
break;
}
}
jumpBuffer.Add(
NavUtil.MultiplyPoint3x4(
math.inverse(localToWorldFromEntity[agent.DestinationSurface].Value),
(float3)lastValidPoint + agent.Offset
)
);
if (jumpBuffer.Length > 0)
{
commandBuffer.RemoveComponent <NavPlanning>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavLerping>(entityInQueryIndex, entity);
}
}
else if (status == PathQueryStatus.Success)
{
pathBuffer.Clear();
agent.PathBufferIndex = 0;
for (int i = 0; i < straightPathCount; ++i)
{
pathBuffer.Add(
NavUtil.MultiplyPoint3x4(
math.inverse(localToWorldFromEntity[surface.Value].Value),
(float3)straightPath[i].position + agent.Offset
)
);
}
if (pathBuffer.Length > 0)
{
commandBuffer.RemoveComponent <NavPlanning>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavLerping>(entityInQueryIndex, entity);
}
}
polygonIdArray.Dispose();
straightPath.Dispose();
straightPathFlags.Dispose();
vertexSide.Dispose();
})
.WithName("NavPlanJob")
.ScheduleParallel();
NavMeshWorld.GetDefaultWorld().AddDependency(Dependency);
barrier.AddJobHandleForProducer(Dependency);
}
protected override void OnUpdate()
{
var commandBuffer = barrier.CreateCommandBuffer().AsParallelWriter();
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
var jumpingFromEntity = GetComponentDataFromEntity <NavJumping>(true);
var pathBufferFromEntity = GetBufferFromEntity <NavPathBufferElement>();
var jumpBufferFromEntity = GetBufferFromEntity <NavJumpBufferElement>();
var navMeshQueryPointerArray = World.GetExistingSystem <NavMeshQuerySystem>().PointerArray;
var settings = navSystem.Settings;
Entities
.WithNone <NavProblem>()
.WithAll <NavPlanning, LocalToParent>()
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(jumpingFromEntity)
.WithNativeDisableParallelForRestriction(pathBufferFromEntity)
.WithNativeDisableParallelForRestriction(jumpBufferFromEntity)
.WithNativeDisableParallelForRestriction(navMeshQueryPointerArray)
.ForEach((Entity entity, int entityInQueryIndex, int nativeThreadIndex, ref NavAgent agent, in Parent surface, in NavDestination destination) =>
{
if (
surface.Value.Equals(Entity.Null) ||
agent.DestinationSurface.Equals(Entity.Null) ||
!localToWorldFromEntity.HasComponent(surface.Value) ||
!localToWorldFromEntity.HasComponent(agent.DestinationSurface)
)
{
return;
}
var agentPosition = localToWorldFromEntity[entity].Position;
var worldPosition = agentPosition;
var worldDestination = agent.LocalDestination.ToWorld(localToWorldFromEntity[agent.DestinationSurface]);
var jumping = jumpingFromEntity.HasComponent(entity);
if (jumping)
{
worldPosition = worldDestination;
worldDestination = agentPosition;
}
var navMeshQueryPointer = navMeshQueryPointerArray[nativeThreadIndex];
UnsafeUtility.CopyPtrToStructure(navMeshQueryPointer.Value, out NavMeshQuery navMeshQuery);
var one = new float3(1);
var status = navMeshQuery.BeginFindPath(
navMeshQuery.MapLocation(worldPosition, one * settings.PathSearchMax, agent.TypeID),
navMeshQuery.MapLocation(worldDestination, one * settings.PathSearchMax, agent.TypeID),
NavMesh.AllAreas
);
while (NavUtil.HasStatus(status, PathQueryStatus.InProgress))
{
status = navMeshQuery.UpdateFindPath(
settings.IterationMax,
out int iterationsPerformed
);
}
var customLerp = destination.CustomLerp;
if (!NavUtil.HasStatus(status, PathQueryStatus.Success))
{
commandBuffer.RemoveComponent <NavPlanning>(entityInQueryIndex, entity);
commandBuffer.RemoveComponent <NavDestination>(entityInQueryIndex, entity);
commandBuffer.AddComponent(entityInQueryIndex, entity, new NavProblem
{
Value = status
});
return;
}
navMeshQuery.EndFindPath(out int pathLength);
var polygonIdArray = new NativeArray <PolygonId>(
NavConstants.PATH_NODE_MAX,
Allocator.Temp
);
navMeshQuery.GetPathResult(polygonIdArray);
var len = pathLength + 1;
var straightPath = new NativeArray <NavMeshLocation>(len, Allocator.Temp);
var straightPathFlags = new NativeArray <StraightPathFlags>(len, Allocator.Temp);
var vertexSide = new NativeArray <float>(len, Allocator.Temp);
var straightPathCount = 0;
status = PathUtils.FindStraightPath(
navMeshQuery,
worldPosition,
worldDestination,
polygonIdArray,
pathLength,
ref straightPath,
ref straightPathFlags,
ref vertexSide,
ref straightPathCount,
NavConstants.PATH_NODE_MAX
);
var jumpBuffer = !jumpBufferFromEntity.HasComponent(entity) ? commandBuffer.AddBuffer <NavJumpBufferElement>(entityInQueryIndex, entity) : jumpBufferFromEntity[entity];
var pathBuffer = !pathBufferFromEntity.HasComponent(entity) ? commandBuffer.AddBuffer <NavPathBufferElement>(entityInQueryIndex, entity) : pathBufferFromEntity[entity];
if (jumping)
{
var lastValidPoint = float3.zero;
for (var i = 0; i < straightPath.Length; ++i)
{
if (navMeshQuery.IsValid(straightPath[i].polygon))
{
lastValidPoint = straightPath[i].position;
}
else
{
break;
}
}
jumpBuffer.Add((lastValidPoint + agent.Offset).ToLocal(localToWorldFromEntity[agent.DestinationSurface]));
if (jumpBuffer.Length > 0)
{
commandBuffer.RemoveComponent <NavPlanning>(entityInQueryIndex, entity);
if (customLerp)
{
commandBuffer.AddComponent <NavCustomLerping>(entityInQueryIndex, entity);
}
else
{
commandBuffer.AddComponent <NavJumping>(entityInQueryIndex, entity);
}
}
}
else if (status == PathQueryStatus.Success)
{
if (pathBuffer.Length > 0)
{
pathBuffer.RemoveAt(pathBuffer.Length - 1);
}
for (var i = straightPathCount - 1; i > 0; --i)
{
pathBuffer.Add(
((float3)straightPath[i].position + agent.Offset).ToLocal(localToWorldFromEntity[surface.Value])
);
}
if (pathBuffer.Length > 0)
{
commandBuffer.RemoveComponent <NavPlanning>(entityInQueryIndex, entity);
if (customLerp)
{
commandBuffer.AddComponent <NavCustomLerping>(entityInQueryIndex, entity);
}
else
{
commandBuffer.AddComponent <NavWalking>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavSteering>(entityInQueryIndex, entity);
}
}
}
polygonIdArray.Dispose();
straightPath.Dispose();
straightPathFlags.Dispose();
vertexSide.Dispose();
})