static void HandleCompletePath(ComponentDataFromEntity <LocalToWorld> localToWorldFromEntity, Entity entity, Rotation rotation, ref NavAgent agent, Parent surface, Translation translation, PhysicsWorld physicsWorld, float elapsedSeconds, EntityCommandBuffer.ParallelWriter commandBuffer, int entityInQueryIndex, NavSettings settings)
{
var rayInput = new RaycastInput
{
Start = localToWorldFromEntity[entity].Position + agent.Offset,
End = math.forward(rotation.Value) * settings.ObstacleRaycastDistanceMax,
Filter = new CollisionFilter
{
BelongsTo = NavUtil.ToBitMask(settings.ColliderLayer),
CollidesWith = NavUtil.ToBitMask(settings.ObstacleLayer)
}
};
if (
!surface.Value.Equals(agent.DestinationSurface) &&
!NavUtil.ApproxEquals(translation.Value, agent.LocalDestination, settings.StoppingDistance) &&
!physicsWorld.CastRay(rayInput, out _)
)
{
agent.JumpSeconds = elapsedSeconds;
commandBuffer.RemoveComponent <NavWalking>(entityInQueryIndex, entity);
commandBuffer.RemoveComponent <NavSteering>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavJumping>(entityInQueryIndex, entity);
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
return;
}
commandBuffer.RemoveComponent <NavWalking>(entityInQueryIndex, entity);
commandBuffer.RemoveComponent <NavSteering>(entityInQueryIndex, entity);
commandBuffer.RemoveComponent <NavDestination>(entityInQueryIndex, entity);
}
void FixedUpdate()
{
if (
entity.Equals(Entity.Null) ||
!entityManager.HasComponent <NavAgent>(entity) ||
!entityManager.HasComponent <Parent>(entity) ||
!entityManager.HasComponent <Translation>(entity) ||
!entityManager.HasComponent <Rotation>(entity)
)
{
return;
}
var agent = entityManager.GetComponentData <NavAgent>(entity);
agent.JumpDegrees = JumpDegrees;
agent.JumpGravity = JumpGravity;
agent.JumpSpeedMultiplierX = JumpSpeedMultiplierX;
agent.JumpSpeedMultiplierY = JumpSpeedMultiplierY;
agent.TranslationSpeed = TranslationSpeed;
agent.RotationSpeed = RotationSpeed;
agent.TypeID = NavUtil.GetAgentType(Type);
agent.Offset = Offset;
entityManager.SetComponentData(entity, agent);
if (!lastWorldDestination.Equals(WorldDestination))
{
entityManager.AddComponentData <NavNeedsDestination>(entity, new NavNeedsDestination
{
Destination = WorldDestination,
Teleport = Teleport
});
lastWorldDestination = WorldDestination;
InitializeEntityTransform(); // Reinitialize in case GameObject transform changes in-between pathing.
}
var surfaceEntity = entityManager.GetComponentData <Parent>(entity);
if (surfaceEntity.Value.Equals(Entity.Null) || !entityManager.HasComponent <NavSurface>(surfaceEntity.Value))
{
return;
}
surfaceSystem.GameObjectMapTryGetValue(
entityManager.GetComponentData <NavSurface>(surfaceEntity.Value).TransformInstanceID,
out var surfaceGameObject
);
if (surfaceGameObject == null)
{
return;
}
gameObject.transform.SetParent(surfaceGameObject.transform);
gameObject.transform.localPosition = entityManager.GetComponentData <Translation>(entity).Value / surfaceGameObject.transform.localScale;
gameObject.transform.localRotation = entityManager.GetComponentData <Rotation>(entity).Value;
}
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);
}
void Start()
{
Entity = entityManager.CreateEntity();
entityManager.AddComponentData(Entity, new NavAgent
{
JumpDegrees = JumpDegrees,
JumpGravity = JumpGravity,
JumpSpeedMultiplierX = JumpSpeedMultiplierX,
JumpSpeedMultiplierY = JumpSpeedMultiplierY,
TranslationSpeed = TranslationSpeed,
RotationSpeed = RotationSpeed,
TypeID = NavUtil.GetAgentType(Type),
Offset = Offset
});
InitializeEntityTransform();
}
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 physicsWorld = buildPhysicsWorld.PhysicsWorld;
var settings = navSystem.Settings;
Dependency = JobHandle.CombineDependencies(Dependency, buildPhysicsWorld.GetOutputDependency());
var isDebugging = IsDebugging;
Entities
.WithNone <NavProblem>()
.WithNone <NavPlanning, NavJumping, NavFalling>()
.WithAll <NavWalking, LocalToParent, NavTerrainCapable>()
.WithReadOnly(physicsWorld)
.ForEach((Entity entity, int entityInQueryIndex, ref Translation translation, ref NavAgent agent, in LocalToWorld localToWorld, in Parent surface) =>
{
var rayInput = new RaycastInput
{
Start = localToWorld.Position + agent.Offset,
End = -math.up() * settings.SurfaceRaycastDistanceMax,
Filter = new CollisionFilter()
{
BelongsTo = NavUtil.ToBitMask(settings.ColliderLayer),
CollidesWith = NavUtil.ToBitMask(settings.SurfaceLayer),
}
};
if (physicsWorld.CastRay(rayInput, out RaycastHit hit))
{
if (isDebugging)
{
UnityEngine.Debug.DrawLine(hit.Position, hit.Position + hit.SurfaceNormal * 15, UnityEngine.Color.green);
UnityEngine.Debug.DrawLine(hit.Position, hit.Position + localToWorld.Up * 7, UnityEngine.Color.cyan);
UnityEngine.Debug.DrawLine(hit.Position, hit.Position + localToWorld.Right * 7, UnityEngine.Color.cyan);
UnityEngine.Debug.DrawLine(hit.Position, hit.Position + localToWorld.Forward * 7, UnityEngine.Color.cyan);
}
agent.SurfacePointNormal = hit.SurfaceNormal;
var currentPosition = translation.Value;
currentPosition.y = hit.Position.y + agent.Offset.y;
translation.Value = currentPosition;
}
})
public void Convert(Entity entity, EntityManager dstManager, GameObjectConversionSystem conversionSystem)
{
dstManager.AddComponentData(entity, new NavAgent
{
JumpDegrees = jumpDegrees,
JumpGravity = jumpGravity,
JumpSpeedMultiplierX = jumpSpeedMultiplierX,
JumpSpeedMultiplierY = jumpSpeedMultiplierY,
TranslationSpeed = translationSpeed,
RotationSpeed = rotationSpeed,
TypeID = NavUtil.GetAgentType(type),
Offset = offset
});
dstManager.AddComponent <NavNeedsSurface>(entity);
dstManager.AddComponent <NavFixTranslation>(entity);
if (isTerrainCapable)
{
dstManager.AddComponent <NavTerrainCapable>(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 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);
var fallingFromEntity = GetComponentDataFromEntity <NavFalling>(true);
var jumpingFromEntity = GetComponentDataFromEntity <NavJumping>(true);
var flockingFromEntity = GetComponentDataFromEntity <NavFlocking>(true);
Entities
.WithNone <NavProblem, NavPlanning>()
.WithAll <NavWalking, LocalToParent>()
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(physicsWorld)
.WithReadOnly(jumpingFromEntity)
.WithReadOnly(fallingFromEntity)
.WithReadOnly(flockingFromEntity)
.WithNativeDisableParallelForRestriction(pathBufferFromEntity)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, ref Translation translation, ref NavSteering navSteering, 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, 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;
var heading = math.normalizesafe(pathBuffer[pathBufferIndex].Value - translation.Value);
if (
!jumpingFromEntity.HasComponent(entity) &&
!fallingFromEntity.HasComponent(entity) &&
flockingFromEntity.HasComponent(entity)
)
{
navSteering.AgentAvoidanceSteering.y = navSteering.SeparationSteering.y = navSteering.AlignmentSteering.y = navSteering.CohesionSteering.y = 0;
heading = math.normalizesafe(
heading +
navSteering.AgentAvoidanceSteering +
navSteering.SeparationSteering +
navSteering.AlignmentSteering +
navSteering.CohesionSteering
);
if (!navSteering.CollisionAvoidanceSteering.Equals(float3.zero))
{
heading = math.normalizesafe(heading + navSteering.CollisionAvoidanceSteering);
}
}
navSteering.CurrentHeading = heading;
})
.WithName("NavSteeringJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
buildPhysicsWorld.AddInputDependencyToComplete(Dependency);
var jumpBufferFromEntity = GetBufferFromEntity <NavJumpBufferElement>();
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 destinationSurfaceLocalToWorld = localToWorldFromEntity[agent.DestinationSurface];
var worldDestination = agent.LocalDestination.ToWorld(destinationSurfaceLocalToWorld);
var velocity = math.distance(translation.Value, worldDestination) / (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;
var surfaceLocalToWorld = localToWorldFromEntity[surface.Value];
waypoint = jumpBuffer[0].Value
.ToWorld(destinationSurfaceLocalToWorld)
.ToLocal(surfaceLocalToWorld);
translation.Value.MoveTowards(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 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;
// Below job is needed because Unity.Physics removes the Parent
// component for 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);
})
.WithoutBurst()
.WithName("NavAddParentToSurfaceJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
// Below job is needed so users don't have to manually add the
// Parent and LocalToParent components when spawning agents.
Entities
.WithNone <NavHasProblem, Parent>()
.ForEach((Entity entity, int entityInQueryIndex, in NavAgent agent) =>
{
commandBuffer.AddComponent <Parent>(entityInQueryIndex, entity);
commandBuffer.AddComponent <LocalToParent>(entityInQueryIndex, entity);
})
.WithoutBurst()
.WithName("NavAddParentToAgentJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
// Below job is needed because Unity.Transforms assumes that
// children should be scaled by their surface by automatically
// providing them with a CompositeScale.
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 jumpBufferFromEntity = GetBufferFromEntity <NavJumpBufferElement>();
var map = needsSurfaceMap;
Dependency = JobHandle.CombineDependencies(Dependency, buildPhysicsWorld.GetOutputDependency());
Entities
.WithNone <NavHasProblem, NavFalling, NavJumping>()
.WithAll <NavNeedsSurface, LocalToParent>()
.WithReadOnly(physicsWorld)
.WithNativeDisableParallelForRestriction(jumpBufferFromEntity)
.WithNativeDisableContainerSafetyRestriction(map)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, ref Parent surface, ref Translation translation, in LocalToWorld localToWorld) =>
{
if (
!surface.Value.Equals(Entity.Null) &&
map.TryGetValue(entity.Index, out bool needsSurface) &&
!map.TryAdd(entity.Index, false)
)
{
return;
}
var rayInput = new RaycastInput
{
Start = localToWorld.Position + agent.Offset,
End = -localToWorld.Up * NavConstants.SURFACE_RAYCAST_DISTANCE_MAX,
Filter = new CollisionFilter()
{
BelongsTo = NavUtil.ToBitMask(NavConstants.COLLIDER_LAYER),
CollidesWith = NavUtil.ToBitMask(NavConstants.SURFACE_LAYER),
}
};
if (!physicsWorld.CastRay(rayInput, out RaycastHit hit))
{
if (++agent.SurfaceRaycastCount >= NavConstants.SURFACE_RAYCAST_MAX)
{
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();
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
})
.WithName("NavSurfaceTrackingJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
}
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 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 localDestination = destination.WorldPoint.ToLocal(localToWorldFromEntity[hit.Entity]) + agent.Offset;
if (NavUtil.ApproxEquals(localDestination, 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 = localDestination
});
commandBuffer.RemoveComponent <NavDestination>(entityInQueryIndex, entity);
return;
}
agent.DestinationSurface = physicsWorld.Bodies[hit.RigidBodyIndex].Entity;
agent.LocalDestination = localDestination;
agent.DestinationSeconds = elapsedSeconds;
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
}
})
.WithName("CreateDestinationJob")
.ScheduleParallel();
barrier.AddJobHandleForProducer(Dependency);
buildPhysicsWorld.AddInputDependencyToComplete(Dependency);
}
protected override void OnUpdate()
{
var physicsWorld = buildPhysicsWorld.PhysicsWorld;
var settings = navSystem.Settings;
var flockingSettings = navSystem.FlockingSettings;
var isDebugging = IsDebugging;
Entities
.WithNone <NavProblem>()
.WithNone <NavPlanning, NavJumping, NavFalling>()
.WithAll <NavObstacleSteering>()
.WithAll <NavWalking, NavFlocking, LocalToParent>()
.WithReadOnly(physicsWorld)
.ForEach((Entity entity, int entityInQueryIndex, ref Translation translation, ref NavAgent agent, ref NavSteering steering, in LocalToWorld localToWorld, in Rotation rotation, in Parent surface) =>
{
var averageHitDirection = float3.zero;
var clostestHitDistance = agent.ObstacleAversionDistance;
var hits = 0;
for (var i = 0; i < NUM_RAYS; ++i)
{
var rayModifier = quaternion.AxisAngle(localToWorld.Up, i / (NUM_RAYS - 1) * math.radians(flockingSettings.CollisionCastingAngle * 2 - flockingSettings.CollisionCastingAngle));
var rayDirection = math.mul(math.mul(rotation.Value, rayModifier), math.forward());
if (isDebugging)
{
Debug.DrawRay(localToWorld.Position, rayDirection * agent.ObstacleAversionDistance, Color.cyan);
}
var ray = new RaycastInput
{
Start = localToWorld.Position,
End = localToWorld.Position + rayDirection * agent.ObstacleAversionDistance,
Filter = new CollisionFilter()
{
BelongsTo = NavUtil.ToBitMask(settings.ColliderLayer),
CollidesWith = NavUtil.ToBitMask(settings.ObstacleLayer),
}
};
if (physicsWorld.CastRay(ray, out RaycastHit hit))
{
var distance = math.distance(localToWorld.Position, hit.Position);
clostestHitDistance = math.select(clostestHitDistance, distance, distance < clostestHitDistance);
averageHitDirection += localToWorld.Position - hit.Position;
++hits;
if (isDebugging)
{
Debug.DrawLine(localToWorld.Position, hit.Position, Color.red);
}
}
}
averageHitDirection /= hits;
var scalar = 1 - (clostestHitDistance / agent.ObstacleAversionDistance);
// Steer away from the average hit direction; addiontally, scale the steering vector by the closest hit distance. Otherwise, the entity just bounces off walls and spins out of control.
steering.CollisionAvoidanceSteering = flockingSettings.ObstacleCollisionAvoidanceStrength * math.normalizesafe(averageHitDirection) * scalar;
})
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 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();
})
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);
}
protected override JobHandle OnUpdate(JobHandle inputDeps)
{
var commandBuffer = barrier.CreateCommandBuffer().ToConcurrent();
var defaultBasis = World.GetExistingSystem <NavBasisSystem>().DefaultBasis;
// Below job is needed because Unity.Physics removes the Parent
// component for dynamic bodies.
var addParentJob = 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(entityInQueryIndex, entity, typeof(LocalToParent));
})
.WithoutBurst()
.WithName("NavAddParentToSurfaceJob")
.Schedule(inputDeps);
barrier.AddJobHandleForProducer(addParentJob);
// Below job is needed because Unity.Transforms assumes that
// children should be scaled by their surface by automatically
// providing them with a CompositeScale.
var removeCompositeScaleJob = Entities
.WithAll <CompositeScale>()
.WithAny <NavSurface, NavBasis>()
.ForEach((Entity entity, int entityInQueryIndex) =>
{
commandBuffer.RemoveComponent <CompositeScale>(entityInQueryIndex, entity);
})
.WithName("NavRemoveCompositeScaleJob")
.Schedule(addParentJob);
barrier.AddJobHandleForProducer(removeCompositeScaleJob);
var elapsedSeconds = (float)Time.ElapsedTime;
var physicsWorld = buildPhysicsWorld.PhysicsWorld;
var jumpBufferFromEntity = GetBufferFromEntity <NavJumpBufferElement>();
var localToWorldFromEntity = GetComponentDataFromEntity <LocalToWorld>(true);
var navSurfaceTrackingJob = Entities
.WithNone <NavFalling, NavJumping>()
.WithAll <NavNeedsSurface, LocalToParent>()
.WithReadOnly(localToWorldFromEntity)
.WithReadOnly(physicsWorld)
.WithNativeDisableParallelForRestriction(jumpBufferFromEntity)
.ForEach((Entity entity, int entityInQueryIndex, ref NavAgent agent, ref Parent surface, ref Translation translation) =>
{
if (
!surface.Value.Equals(Entity.Null) &&
needsSurfaceDictionary.GetOrAdd(entity.Index, true)
)
{
return;
}
needsSurfaceDictionary[entity.Index] = false;
var rayInput = new RaycastInput
{
Start = localToWorldFromEntity[entity].Position + agent.Offset,
End = -math.up() * NavConstants.SURFACE_RAYCAST_DISTANCE_MAX,
Filter = new CollisionFilter()
{
BelongsTo = NavUtil.ToBitMask(NavConstants.COLLIDER_LAYER),
CollidesWith = NavUtil.ToBitMask(NavConstants.SURFACE_LAYER),
}
};
if (!physicsWorld.CastRay(rayInput, out RaycastHit hit) || hit.RigidBodyIndex == -1)
{
if (++agent.SurfaceRaycastCount >= NavConstants.SURFACE_RAYCAST_MAX)
{
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);
if (!jumpBufferFromEntity.Exists(entity))
{
return;
}
var jumpBuffer = jumpBufferFromEntity[entity];
if (jumpBuffer.Length < 1)
{
return;
}
translation.Value = jumpBuffer[0].Value + agent.Offset;
jumpBuffer.Clear();
commandBuffer.AddComponent <NavPlanning>(entityInQueryIndex, entity);
})
.WithoutBurst()
.WithName("NavSurfaceTrackingJob")
.Schedule(
JobHandle.CombineDependencies(
removeCompositeScaleJob,
buildPhysicsWorld.FinalJobHandle
)
);
barrier.AddJobHandleForProducer(navSurfaceTrackingJob);
return(navSurfaceTrackingJob);
}