protected override JobHandle OnUpdate(JobHandle inputDeps)
{
var commandBuffer = barrier.CreateCommandBuffer().ToConcurrent();
var physicsWorld = buildPhysicsWorld.PhysicsWorld;
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
var createJob = Entities
.WithChangeFilter <NavNeedsDestination>()
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(physicsWorld)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, in NavNeedsDestination destination) =>
{
var collider = SphereCollider.Create(
new SphereGeometry()
{
Center = destination.Value,
Radius = 1
},
new CollisionFilter()
{
BelongsTo = NavUtil.ToBitMask(NavConstants.COLLIDER_LAYER),
CollidesWith = NavUtil.ToBitMask(NavConstants.SURFACE_LAYER),
}
);
unsafe
{
var castInput = new ColliderCastInput()
{
Collider = (Collider *)collider.GetUnsafePtr(),
Orientation = quaternion.identity
};
if (!physicsWorld.CastCollider(castInput, out ColliderCastHit hit) || hit.RigidBodyIndex == -1)
{
commandBuffer.RemoveComponent <NavNeedsDestination>(entityInQueryIndex, entity); // Ignore invalid destinations.
return;
}
agent.DestinationSurface = physicsWorld.Bodies[hit.RigidBodyIndex].Entity;
agent.LocalDestination = NavUtil.MultiplyPoint3x4(
math.inverse(localToWorldFromEntity[agent.DestinationSurface].Value),
destination.Value
) + agent.Offset;
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
}
})
.WithName("CreateDestinationJob")
.Schedule(JobHandle.CombineDependencies(
inputDeps,
buildPhysicsWorld.FinalJobHandle
));
barrier.AddJobHandleForProducer(createJob);
return(createJob);
}
public void ExecuteNext(int firstIndex, int index)
{
if (index > NavConstants.AGENTS_PER_CELL_MAX)
{
return;
}
var agent = AgentFromEntity[AgentEntityArray[index]];
if (agent.Surface.Equals(Entity.Null) || !ParentFromEntity.Exists(agent.Surface))
{
return;
}
var basis = ParentFromEntity[agent.Surface].Value;
if (basis.Equals(Entity.Null) || !LocalToWorldFromEntity.Exists(basis))
{
return;
}
var basisTransform = LocalToWorldFromEntity[basis].Value;
var pathBuffer = PathBufferFromEntity[AgentEntityArray[index]];
if (pathBuffer.Length == 0 || agent.PathBufferIndex >= pathBuffer.Length)
{
return;
}
var rotation = RotationFromEntity[AgentEntityArray[index]];
float3 avoidanceDestination = index % 2 == 1 ? Vector3.right : Vector3.left;
avoidanceDestination = (Quaternion)rotation.Value * ((float3)Vector3.forward + avoidanceDestination) * agent.TranslationSpeed * DeltaSeconds;
avoidanceDestination += pathBuffer[agent.PathBufferIndex];
agent.AvoidanceDestination = NavUtil.MultiplyPoint3x4(basisTransform, avoidanceDestination - agent.Offset);
AgentFromEntity[AgentEntityArray[index]] = agent;
CommandBuffer.RemoveComponent <NavLerping>(index, AgentEntityArray[index]);
CommandBuffer.AddComponent <NavPlanning>(index, AgentEntityArray[index]);
CommandBuffer.AddComponent <NavAvoidant>(index, AgentEntityArray[index]);
}
protected override void OnUpdate()
{
var commandBuffer = barrier.CreateCommandBuffer().AsParallelWriter();
var elapsedSeconds = (float)Time.ElapsedTime;
var deltaSeconds = Time.DeltaTime;
var physicsWorld = buildPhysicsWorld.PhysicsWorld;
var settings = navSystem.Settings;
var pathBufferFromEntity = GetBufferFromEntity <NavPathBufferElement>();
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
Entities
.WithNone <NavProblem, NavPlanning>()
.WithAll <NavWalking, LocalToParent>()
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(physicsWorld)
.WithNativeDisableParallelForRestriction(pathBufferFromEntity)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, ref Translation translation, ref Rotation rotation, in Parent surface) =>
{
if (!pathBufferFromEntity.HasComponent(entity) || agent.DestinationSurface.Equals(Entity.Null))
{
return;
}
var pathBuffer = pathBufferFromEntity[entity];
if (pathBuffer.Length == 0)
{
HandleCompletePath(localToWorldFromEntity, entity, rotation, ref agent, ref pathBuffer, surface, translation, physicsWorld, elapsedSeconds, commandBuffer, entityInQueryIndex, settings);
return;
}
var pathBufferIndex = pathBuffer.Length - 1;
if (NavUtil.ApproxEquals(translation.Value, pathBuffer[pathBufferIndex].Value, settings.StoppingDistance))
{
pathBuffer.RemoveAt(pathBufferIndex);
}
if (pathBuffer.Length == 0)
{
return;
}
pathBufferIndex = pathBuffer.Length - 1;
translation.Value = Vector3.MoveTowards(translation.Value, pathBuffer[pathBufferIndex].Value, agent.TranslationSpeed * deltaSeconds);
var lookAt = NavUtil.MultiplyPoint3x4( // To world (from local in terms of destination surface).
localToWorldFromEntity[agent.DestinationSurface].Value,
pathBuffer[pathBufferIndex].Value
);
lookAt = NavUtil.MultiplyPoint3x4( // To local (in terms of agent's current surface).
math.inverse(localToWorldFromEntity[surface.Value].Value),
lookAt
);
lookAt.y = translation.Value.y;
var lookRotation = quaternion.LookRotationSafe(lookAt - translation.Value, math.up());
if (math.length(agent.SurfacePointNormal) > 0.01f)
{
lookRotation = Quaternion.FromToRotation(math.up(), agent.SurfacePointNormal) * lookRotation;
}
rotation.Value = math.slerp(rotation.Value, lookRotation, deltaSeconds / agent.RotationSpeed);
})
.WithName("NavWalkJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
buildPhysicsWorld.AddInputDependency(Dependency);
var jumpBufferFromEntity = GetBufferFromEntity <NavJumpBufferElement>();
var fallingFromEntity = GetComponentDataFromEntity <NavFalling>();
Entities
.WithNone <NavProblem>()
.WithAny <NavFalling, NavJumping>()
.WithAll <LocalToParent>()
.WithReadOnly(fallingFromEntity)
.WithReadOnly(jumpBufferFromEntity)
.WithReadOnly(localToWorldFromEntity)
.ForEach((Entity entity, int entityInQueryIndex, ref Translation translation, in NavAgent agent, in Parent surface) =>
{
if (agent.DestinationSurface.Equals(Entity.Null))
{
return;
}
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
if (!jumpBufferFromEntity.HasComponent(entity))
{
return;
}
var jumpBuffer = jumpBufferFromEntity[entity];
if (jumpBuffer.Length == 0 && !fallingFromEntity.HasComponent(entity))
{
return;
}
var destination = NavUtil.MultiplyPoint3x4(
localToWorldFromEntity[agent.DestinationSurface].Value,
agent.LocalDestination
);
var velocity = Vector3.Distance(translation.Value, destination) / (math.sin(2 * math.radians(agent.JumpDegrees)) / agent.JumpGravity);
var yVelocity = math.sqrt(velocity) * math.sin(math.radians(agent.JumpDegrees));
var waypoint = translation.Value + math.up() * float.NegativeInfinity;
if (!fallingFromEntity.HasComponent(entity))
{
var xVelocity = math.sqrt(velocity) * math.cos(math.radians(agent.JumpDegrees)) * agent.JumpSpeedMultiplierX;
waypoint = NavUtil.MultiplyPoint3x4( // To world (from local in terms of destination surface).
localToWorldFromEntity[agent.DestinationSurface].Value,
jumpBuffer[0].Value
);
waypoint = NavUtil.MultiplyPoint3x4( // To local (in terms of agent's current surface).
math.inverse(localToWorldFromEntity[surface.Value].Value),
waypoint
);
translation.Value = Vector3.MoveTowards(translation.Value, waypoint, xVelocity * deltaSeconds);
}
translation.Value.y += (yVelocity - (elapsedSeconds - agent.JumpSeconds) * agent.JumpGravity) * deltaSeconds * agent.JumpSpeedMultiplierY;
if (elapsedSeconds - agent.JumpSeconds >= settings.JumpSecondsMax)
{
commandBuffer.RemoveComponent <NavJumping>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavFalling>(entityInQueryIndex, entity);
}
if (!NavUtil.ApproxEquals(translation.Value, waypoint, 1))
{
return;
}
commandBuffer.AddComponent <NavNeedsSurface>(entityInQueryIndex, entity);
commandBuffer.RemoveComponent <NavJumping>(entityInQueryIndex, entity);
})
protected override void OnUpdate()
{
var commandBuffer = barrier.CreateCommandBuffer().AsParallelWriter();
var elapsedSeconds = (float)Time.ElapsedTime;
var deltaSeconds = Time.DeltaTime;
var physicsWorld = buildPhysicsWorld.PhysicsWorld;
var pathBufferFromEntity = GetBufferFromEntity <NavPathBufferElement>(true);
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
Dependency = JobHandle.CombineDependencies(Dependency, buildPhysicsWorld.GetOutputDependency());
Entities
.WithNone <NavHasProblem, NavPlanning, NavJumping>()
.WithAll <NavLerping, LocalToParent>()
.WithReadOnly(pathBufferFromEntity)
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(physicsWorld)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, ref Translation translation, ref Rotation rotation, in Parent surface) =>
{
if (!pathBufferFromEntity.HasComponent(entity) || agent.DestinationSurface.Equals(Entity.Null))
{
return;
}
var pathBuffer = pathBufferFromEntity[entity];
if (pathBuffer.Length == 0)
{
return;
}
if (agent.PathBufferIndex >= pathBuffer.Length)
{
--agent.PathBufferIndex;
return;
}
var localDestination = agent.LocalDestination;
var localWaypoint = pathBuffer[agent.PathBufferIndex].Value;
if (
NavUtil.ApproxEquals(translation.Value, localWaypoint, 1) &&
++agent.PathBufferIndex > pathBuffer.Length - 1
)
{
var rayInput = new RaycastInput
{
Start = localToWorldFromEntity[entity].Position + agent.Offset,
End = math.forward(rotation.Value) * NavConstants.OBSTACLE_RAYCAST_DISTANCE_MAX,
Filter = new CollisionFilter
{
BelongsTo = NavUtil.ToBitMask(NavConstants.COLLIDER_LAYER),
CollidesWith = NavUtil.ToBitMask(NavConstants.OBSTACLE_LAYER)
}
};
if (
!surface.Value.Equals(agent.DestinationSurface) &&
!NavUtil.ApproxEquals(translation.Value, localDestination, 1) &&
!physicsWorld.CastRay(rayInput, out RaycastHit hit)
)
{
agent.JumpSeconds = elapsedSeconds;
commandBuffer.AddComponent <NavJumping>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
return;
}
commandBuffer.RemoveComponent <NavLerping>(entityInQueryIndex, entity);
commandBuffer.RemoveComponent <NavNeedsDestination>(entityInQueryIndex, entity);
agent.PathBufferIndex = 0;
return;
}
translation.Value = Vector3.MoveTowards(translation.Value, localWaypoint, agent.TranslationSpeed * deltaSeconds);
var lookAt = NavUtil.MultiplyPoint3x4( // To world (from local in terms of destination surface).
localToWorldFromEntity[agent.DestinationSurface].Value,
localWaypoint
);
lookAt = NavUtil.MultiplyPoint3x4( // To local (in terms of agent's current surface).
math.inverse(localToWorldFromEntity[surface.Value].Value),
lookAt
);
lookAt.y = translation.Value.y;
var lookRotation = quaternion.LookRotationSafe(lookAt - translation.Value, math.up());
if (math.length(agent.SurfacePointNormal) > 0.01f)
{
lookRotation = Quaternion.FromToRotation(math.up(), agent.SurfacePointNormal) * lookRotation;
}
rotation.Value = math.slerp(rotation.Value, lookRotation, deltaSeconds / agent.RotationSpeed);
})
.WithName("NavWalkJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
var jumpBufferFromEntity = GetBufferFromEntity <NavJumpBufferElement>();
var fallingFromEntity = GetComponentDataFromEntity <NavFalling>();
Entities
.WithNone <NavHasProblem>()
.WithAny <NavFalling, NavJumping>()
.WithAll <LocalToParent>()
.WithReadOnly(fallingFromEntity)
.WithReadOnly(jumpBufferFromEntity)
.WithReadOnly(localToWorldFromEntity)
.ForEach((Entity entity, int entityInQueryIndex, ref Translation translation, in NavAgent agent, in Parent surface) =>
{
if (agent.DestinationSurface.Equals(Entity.Null))
{
return;
}
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
if (!jumpBufferFromEntity.HasComponent(entity))
{
return;
}
var jumpBuffer = jumpBufferFromEntity[entity];
if (jumpBuffer.Length == 0 && !fallingFromEntity.HasComponent(entity))
{
return;
}
var destination = NavUtil.MultiplyPoint3x4(
localToWorldFromEntity[agent.DestinationSurface].Value,
agent.LocalDestination
);
var velocity = Vector3.Distance(translation.Value, destination) / (math.sin(2 * math.radians(agent.JumpDegrees)) / agent.JumpGravity);
var yVelocity = math.sqrt(velocity) * math.sin(math.radians(agent.JumpDegrees));
var waypoint = translation.Value + math.up() * float.NegativeInfinity;
if (!fallingFromEntity.HasComponent(entity))
{
var xVelocity = math.sqrt(velocity) * math.cos(math.radians(agent.JumpDegrees)) * agent.JumpSpeedMultiplierX;
waypoint = NavUtil.MultiplyPoint3x4( // To world (from local in terms of destination surface).
localToWorldFromEntity[agent.DestinationSurface].Value,
jumpBuffer[0].Value
);
waypoint = NavUtil.MultiplyPoint3x4( // To local (in terms of agent's current surface).
math.inverse(localToWorldFromEntity[surface.Value].Value),
waypoint
);
translation.Value = Vector3.MoveTowards(translation.Value, waypoint, xVelocity * deltaSeconds);
}
translation.Value.y += (yVelocity - (elapsedSeconds - agent.JumpSeconds) * agent.JumpGravity) * deltaSeconds * agent.JumpSpeedMultiplierY;
if (elapsedSeconds - agent.JumpSeconds >= NavConstants.JUMP_SECONDS_MAX)
{
commandBuffer.RemoveComponent <NavJumping>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavFalling>(entityInQueryIndex, entity);
}
if (!NavUtil.ApproxEquals(translation.Value, waypoint, 1))
{
return;
}
commandBuffer.AddComponent <NavNeedsSurface>(entityInQueryIndex, entity);
commandBuffer.RemoveComponent <NavJumping>(entityInQueryIndex, entity);
})
protected override void OnUpdate()
{
var commandBuffer = barrier.CreateCommandBuffer().AsParallelWriter();
var defaultBasis = World.GetExistingSystem <NavBasisSystem>().DefaultBasis;
// Prevents Unity.Physics from removing the Parent component from dynamic bodies:
Entities
.WithNone <Parent>()
.ForEach((Entity entity, int entityInQueryIndex, in NavSurface surface) =>
{
if (surface.Basis.Equals(Entity.Null))
{
commandBuffer.AddComponent(entityInQueryIndex, entity, new Parent
{
Value = defaultBasis
});
}
else
{
commandBuffer.AddComponent(entityInQueryIndex, entity, new Parent
{
Value = surface.Basis
});
}
commandBuffer.AddComponent <LocalToParent>(entityInQueryIndex, entity);
})
.WithName("NavAddParentToSurfaceJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
// Adds Parent and LocalToParent components when to agents:
Entities
.WithNone <NavProblem, Parent>()
.ForEach((Entity entity, int entityInQueryIndex, in NavAgent agent) =>
{
commandBuffer.AddComponent <Parent>(entityInQueryIndex, entity);
commandBuffer.AddComponent <LocalToParent>(entityInQueryIndex, entity);
})
.WithName("NavAddParentToAgentJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
// Prevents Unity.Transforms from assuming that children should be scaled by their parent:
Entities
.WithAll <CompositeScale>()
.WithAny <NavSurface, NavBasis>()
.ForEach((Entity entity, int entityInQueryIndex) =>
{
commandBuffer.RemoveComponent <CompositeScale>(entityInQueryIndex, entity);
})
.WithName("NavRemoveCompositeScaleJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
var elapsedSeconds = (float)Time.ElapsedTime;
var physicsWorld = buildPhysicsWorld.PhysicsWorld;
var settings = navSystem.Settings;
var jumpBufferFromEntity = GetBufferFromEntity <NavJumpBufferElement>();
var pathBufferFromEntity = GetBufferFromEntity <NavPathBufferElement>();
Dependency = JobHandle.CombineDependencies(Dependency, buildPhysicsWorld.GetOutputDependency());
Entities
.WithNone <NavProblem, NavFalling, NavJumping>()
.WithAll <NavNeedsSurface, LocalToParent>()
.WithReadOnly(physicsWorld)
.WithNativeDisableParallelForRestriction(jumpBufferFromEntity)
.WithNativeDisableParallelForRestriction(pathBufferFromEntity)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, ref Parent surface, ref Translation translation, in LocalToWorld localToWorld) =>
{
if (!surface.Value.Equals(Entity.Null) && false)
{
return;
}
var rayInput = new RaycastInput
{
Start = localToWorld.Position + agent.Offset,
End = -localToWorld.Up * settings.SurfaceRaycastDistanceMax,
Filter = new CollisionFilter()
{
BelongsTo = NavUtil.ToBitMask(settings.ColliderLayer),
CollidesWith = NavUtil.ToBitMask(settings.SurfaceLayer),
}
};
if (!physicsWorld.CastRay(rayInput, out RaycastHit hit))
{
if (++agent.SurfaceRaycastCount >= settings.SurfaceRaycastMax)
{
agent.FallSeconds = elapsedSeconds;
commandBuffer.RemoveComponent <NavNeedsSurface>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavFalling>(entityInQueryIndex, entity);
}
return;
}
agent.SurfaceRaycastCount = 0;
surface.Value = physicsWorld.Bodies[hit.RigidBodyIndex].Entity;
commandBuffer.RemoveComponent <NavNeedsSurface>(entityInQueryIndex, entity);
translation.Value.y = hit.Position.y + agent.Offset.y;
if (!jumpBufferFromEntity.HasComponent(entity))
{
return;
}
var jumpBuffer = jumpBufferFromEntity[entity];
if (jumpBuffer.Length < 1)
{
return;
}
translation.Value = jumpBuffer[0].Value + agent.Offset;
jumpBuffer.Clear();
if (pathBufferFromEntity.HasComponent(entity))
{
pathBufferFromEntity[entity].Clear();
}
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
})
.WithName("NavSurfaceTrackingJob")
.ScheduleParallel();
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
// TODO : Move following jobs and related code into transform extensions package. //
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
// Corrects the translation of children with a parent not at the origin:
Entities
.WithChangeFilter <PreviousParent>()
.WithAny <NavFixTranslation>()
.WithReadOnly(localToWorldFromEntity)
.ForEach((Entity entity, int entityInQueryIndex, ref Translation translation, in PreviousParent previousParent, in Parent parent) =>
{
if (previousParent.Value.Equals(Entity.Null) || !localToWorldFromEntity.HasComponent(parent.Value))
{
return;
}
var parentTransform = localToWorldFromEntity[parent.Value];
if (parentTransform.Position.Equals(float3.zero))
{
commandBuffer.RemoveComponent <NavFixTranslation>(entityInQueryIndex, entity);
return;
}
translation.Value = NavUtil.MultiplyPoint3x4(
math.inverse(parentTransform.Value),
translation.Value
);
commandBuffer.RemoveComponent <NavFixTranslation>(entityInQueryIndex, entity);
})
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 JobHandle OnUpdate(JobHandle inputDeps)
{
var commandBuffer = barrier.CreateCommandBuffer().ToConcurrent();
var elapsedSeconds = (float)Time.ElapsedTime;
var deltaSeconds = Time.DeltaTime;
var physicsWorld = buildPhysicsWorldSystem.PhysicsWorld;
var pathBufferFromEntity = GetBufferFromEntity <NavPathBufferElement>(true);
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
var avoidantFromEntity = GetComponentDataFromEntity <NavAvoidant>(true);
var walkJob = Entities
.WithNone <NavPlanning, NavJumping>()
.WithAll <NavLerping, Parent, LocalToParent>()
.WithReadOnly(pathBufferFromEntity)
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(avoidantFromEntity)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, ref Translation translation, ref Rotation rotation) =>
{
var pathBuffer = pathBufferFromEntity[entity];
if (pathBuffer.Length == 0)
{
return;
}
if (agent.PathBufferIndex >= pathBuffer.Length)
{
--agent.PathBufferIndex;
return;
}
var destination = pathBuffer[agent.PathBufferIndex].Value;
if (!agent.DestinationSurface.Equals(Entity.Null))
{
agent.WorldDestination = NavUtil.MultiplyPoint3x4(
localToWorldFromEntity[agent.DestinationSurface].Value,
agent.LocalDestination
);
}
var avoidant = avoidantFromEntity.Exists(entity);
var worldDestination = avoidant ? agent.AvoidanceDestination : agent.WorldDestination;
var worldPosition = localToWorldFromEntity[entity].Position;
if (
NavUtil.ApproxEquals(translation.Value, destination, 1) &&
++agent.PathBufferIndex > pathBuffer.Length - 1
)
{
if (!avoidant)
{
var rayInput = new RaycastInput
{
Start = translation.Value,
End = math.forward(rotation.Value) * NavConstants.OBSTACLE_RAYCAST_DISTANCE_MAX,
Filter = CollisionFilter.Default
};
if (
!physicsWorld.CastRay(rayInput, out RaycastHit hit) &&
!NavUtil.ApproxEquals(worldPosition, worldDestination, 1)
)
{
agent.JumpSeconds = elapsedSeconds;
commandBuffer.AddComponent <NavJumping>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
return;
}
}
commandBuffer.RemoveComponent <NavLerping>(entityInQueryIndex, entity);
if (avoidant)
{
commandBuffer.RemoveComponent <NavAvoidant>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
}
agent.PathBufferIndex = 0;
return;
}
var lookAt = worldDestination;
lookAt.y = worldPosition.y;
rotation.Value = quaternion.LookRotationSafe(lookAt - worldPosition, math.up());
translation.Value = Vector3.MoveTowards(translation.Value, destination, agent.TranslationSpeed * deltaSeconds);
agent.LastDestination = agent.WorldDestination;
})
.WithName("NavWalkJob")
.Schedule(
JobHandle.CombineDependencies(
inputDeps,
buildPhysicsWorldSystem.FinalJobHandle
)
);
var jumpBufferFromEntity = GetBufferFromEntity <NavJumpBufferElement>();
var fallingFromEntity = GetComponentDataFromEntity <NavFalling>();
var artificialGravityJob = Entities
.WithAny <NavFalling, NavJumping>()
.WithAll <Parent, LocalToParent>()
.WithReadOnly(fallingFromEntity)
.WithNativeDisableParallelForRestriction(jumpBufferFromEntity)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, ref Translation translation) =>
{
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity); // For sticking the landing.
var jumpBuffer = jumpBufferFromEntity[entity];
if (jumpBuffer.Length == 0 && !fallingFromEntity.Exists(entity))
{
return;
}
var velocity = Vector3.Distance(translation.Value, agent.WorldDestination) / (math.sin(2 * math.radians(agent.JumpDegrees)) / agent.JumpGravity);
var yVelocity = math.sqrt(velocity) * math.sin(math.radians(agent.JumpDegrees));
var destination = translation.Value + math.up() * float.NegativeInfinity;
if (!fallingFromEntity.Exists(entity))
{
var xVelocity = math.sqrt(velocity) * math.cos(math.radians(agent.JumpDegrees));
destination = jumpBuffer[0].Value;
translation.Value = Vector3.MoveTowards(translation.Value, destination, xVelocity * deltaSeconds);
}
translation.Value.y += (yVelocity - (elapsedSeconds - agent.JumpSeconds) * agent.JumpGravity) * deltaSeconds;
if (elapsedSeconds - agent.JumpSeconds >= NavConstants.JUMP_SECONDS_MAX)
{
commandBuffer.RemoveComponent <NavJumping>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavFalling>(entityInQueryIndex, entity);
}
if (!NavUtil.ApproxEquals(translation.Value, destination, 1))
{
return;
}
commandBuffer.AddComponent <NavNeedsSurface>(entityInQueryIndex, entity);
commandBuffer.RemoveComponent <NavJumping>(entityInQueryIndex, entity);
jumpBuffer.Clear();
})
.WithName("NavArtificialGravityJob")
.Schedule(walkJob);
barrier.AddJobHandleForProducer(artificialGravityJob);
return(artificialGravityJob);
}
protected override void OnUpdate()
{
var commandBuffer = barrier.CreateCommandBuffer().AsParallelWriter();
var elapsedSeconds = (float)Time.ElapsedTime;
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
var physicsWorld = buildPhysicsWorld.PhysicsWorld;
var settings = navSystem.Settings;
Entities
.WithNone <NavProblem>()
.WithChangeFilter <NavDestination>()
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(physicsWorld)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, in NavDestination destination) =>
{
if (elapsedSeconds - agent.DestinationSeconds < settings.DestinationRateLimitSeconds)
{
commandBuffer.AddComponent <NavDestination>(entityInQueryIndex, entity, destination); // So that the change filter applies next frame.
return;
}
var collider = SphereCollider.Create(
new SphereGeometry()
{
Center = destination.WorldPoint,
Radius = settings.DestinationSurfaceColliderRadius
},
new CollisionFilter()
{
BelongsTo = NavUtil.ToBitMask(settings.ColliderLayer),
CollidesWith = NavUtil.ToBitMask(settings.SurfaceLayer),
}
);
unsafe
{
var castInput = new ColliderCastInput()
{
Collider = (Collider *)collider.GetUnsafePtr(),
Orientation = quaternion.identity
};
if (!physicsWorld.CastCollider(castInput, out ColliderCastHit hit))
{
commandBuffer.RemoveComponent <NavDestination>(entityInQueryIndex, entity); // Ignore invalid destinations.
return;
}
var destinationPoint = NavUtil.MultiplyPoint3x4(
math.inverse(localToWorldFromEntity[hit.Entity].Value),
destination.WorldPoint
) + agent.Offset;
if (NavUtil.ApproxEquals(destinationPoint, agent.LocalDestination, destination.Tolerance))
{
return;
}
if (destination.Teleport)
{
commandBuffer.SetComponent <Parent>(entityInQueryIndex, entity, new Parent
{
Value = hit.Entity
});
commandBuffer.SetComponent <Translation>(entityInQueryIndex, entity, new Translation
{
Value = destinationPoint
});
commandBuffer.RemoveComponent <NavDestination>(entityInQueryIndex, entity);
return;
}
agent.DestinationSurface = physicsWorld.Bodies[hit.RigidBodyIndex].Entity;
agent.LocalDestination = destinationPoint;
agent.DestinationSeconds = elapsedSeconds;
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
}
})
.WithName("CreateDestinationJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
buildPhysicsWorld.AddInputDependency(Dependency);
}
protected override void OnUpdate()
{
var commandBuffer = barrier.CreateCommandBuffer().AsParallelWriter();
var physicsWorld = buildPhysicsWorld.PhysicsWorld;
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
Dependency = JobHandle.CombineDependencies(Dependency, buildPhysicsWorld.GetOutputDependency());
Entities
.WithNone <NavHasProblem>()
.WithChangeFilter <NavNeedsDestination>()
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(physicsWorld)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, in NavNeedsDestination needsDestination) =>
{
var collider = SphereCollider.Create(
new SphereGeometry()
{
Center = needsDestination.Destination,
Radius = NavConstants.DESTINATION_SURFACE_COLLIDER_RADIUS
},
new CollisionFilter()
{
BelongsTo = NavUtil.ToBitMask(NavConstants.COLLIDER_LAYER),
CollidesWith = NavUtil.ToBitMask(NavConstants.SURFACE_LAYER),
}
);
unsafe
{
var castInput = new ColliderCastInput()
{
Collider = (Collider *)collider.GetUnsafePtr(),
Orientation = quaternion.identity
};
if (!physicsWorld.CastCollider(castInput, out ColliderCastHit hit))
{
commandBuffer.RemoveComponent <NavNeedsDestination>(entityInQueryIndex, entity); // Ignore invalid destinations.
return;
}
var destination = NavUtil.MultiplyPoint3x4(
math.inverse(localToWorldFromEntity[hit.Entity].Value),
needsDestination.Destination
) + agent.Offset;
if (needsDestination.Teleport)
{
commandBuffer.SetComponent <Parent>(entityInQueryIndex, entity, new Parent
{
Value = hit.Entity
});
commandBuffer.SetComponent <Translation>(entityInQueryIndex, entity, new Translation
{
Value = destination
});
commandBuffer.RemoveComponent <NavNeedsDestination>(entityInQueryIndex, entity);
return;
}
agent.DestinationSurface = physicsWorld.Bodies[hit.RigidBodyIndex].Entity;
agent.LocalDestination = destination;
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
}
})
.WithName("CreateDestinationJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
}