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