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