-
Notifications
You must be signed in to change notification settings - Fork 3
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add initial PathFinder and assorted scribbles
- Loading branch information
Showing
4 changed files
with
255 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,183 @@ | ||
using System; | ||
using System.Buffers; | ||
using System.Collections.Frozen; | ||
using System.Collections.Generic; | ||
using System.Numerics; | ||
using zzio; | ||
using zzio.scn; | ||
|
||
namespace zzre.game; | ||
|
||
public class PathFinder : IDisposable | ||
{ | ||
public const uint InvalidId = uint.MaxValue; | ||
|
||
private readonly Random random = Random.Shared; | ||
private readonly WorldCollider collider; | ||
private readonly WaypointSystem wpSystem; | ||
private readonly FrozenDictionary<uint, int> idToIndex; | ||
private readonly bool[] isVisible; | ||
private bool disposedValue; | ||
|
||
public int WaypointCount => wpSystem.Waypoints.Length; | ||
|
||
public PathFinder(WaypointSystem wpSystem, WorldCollider collider) | ||
{ | ||
this.collider = collider; | ||
this.wpSystem = wpSystem; | ||
idToIndex = wpSystem.Waypoints | ||
.Indexed() | ||
.ToFrozenDictionary(t => t.Value.Id, t => t.Index); | ||
|
||
isVisible = ArrayPool<bool>.Shared.Rent(WaypointCount * WaypointCount); | ||
if (WaypointCount > 0) | ||
{ | ||
if (wpSystem.Waypoints[0].VisibleIds is null) | ||
RaycastVisibility(); | ||
else | ||
SetVisibilityFromData(); | ||
} | ||
} | ||
|
||
protected virtual void Dispose(bool disposing) | ||
{ | ||
if (!disposedValue) | ||
{ | ||
if (disposing) | ||
{ | ||
ArrayPool<bool>.Shared.Return(isVisible); | ||
} | ||
disposedValue = true; | ||
} | ||
} | ||
|
||
public void Dispose() | ||
{ | ||
// Do not change this code. Put cleanup code in 'Dispose(bool disposing)' method | ||
Dispose(disposing: true); | ||
GC.SuppressFinalize(this); | ||
} | ||
|
||
private void RaycastVisibility() | ||
{ | ||
Array.Fill(isVisible, false); | ||
for (int i = 0; i < WaypointCount; i++) | ||
{ | ||
isVisible[i * WaypointCount + i] = true; | ||
for (int j = i + 1; j < WaypointCount; j++) | ||
{ | ||
if (collider.Intersects(new Line(wpSystem.Waypoints[i].Position, wpSystem.Waypoints[j].Position))) | ||
{ | ||
isVisible[i * WaypointCount + j] = true; | ||
isVisible[j * WaypointCount + i] = true; | ||
} | ||
} | ||
} | ||
} | ||
|
||
private void SetVisibilityFromData() | ||
{ | ||
Array.Fill(isVisible, false); | ||
for (int i = 0; i < WaypointCount; i++) | ||
{ | ||
var visibleIds = wpSystem.Waypoints[i].VisibleIds; | ||
foreach (var id in visibleIds ?? []) | ||
{ | ||
var j = idToIndex[id]; | ||
isVisible[i * WaypointCount + j] = true; | ||
} | ||
} | ||
} | ||
|
||
public bool IsVisible(uint fromId, uint toId) => isVisible[fromId * WaypointCount + toId]; | ||
|
||
public Vector3 this[uint id] => wpSystem.Waypoints[idToIndex[id]].Position; | ||
|
||
public interface IWaypointFilter | ||
{ | ||
bool IsValid(in Waypoint waypoint) => true; | ||
bool IsValid(in Waypoint waypoint, int index) => IsValid(waypoint); | ||
} | ||
|
||
public uint NearestId<TFilter>(Vector3 position, TFilter filter) | ||
where TFilter : struct, IWaypointFilter | ||
{ | ||
// TODO: Investigate acceleration structure for searching nearest waypoint | ||
float bestDistanceSqr = float.PositiveInfinity; | ||
uint bestId = InvalidId; | ||
for (int i = 0; i < WaypointCount; i++) | ||
{ | ||
ref readonly var waypoint = ref wpSystem.Waypoints[i]; | ||
if (!filter.IsValid(waypoint, i)) | ||
continue; | ||
|
||
var curDistanceSqr = Vector3.DistanceSquared(waypoint.Position, position); | ||
if (curDistanceSqr < bestDistanceSqr) | ||
{ | ||
bestDistanceSqr = curDistanceSqr; | ||
bestId = waypoint.Id; | ||
} | ||
} | ||
return bestId; | ||
} | ||
|
||
public uint FurthestId<TFilter>(Vector3 position, TFilter filter) | ||
where TFilter : IWaypointFilter | ||
{ | ||
float bestDistanceSqr = float.NegativeInfinity; | ||
uint bestId = InvalidId; | ||
for (int i = 0; i < WaypointCount; i++) | ||
{ | ||
ref readonly var waypoint = ref wpSystem.Waypoints[i]; | ||
if (!filter.IsValid(waypoint, i)) | ||
continue; | ||
|
||
var curDistanceSqr = Vector3.DistanceSquared(waypoint.Position, position); | ||
if (curDistanceSqr > bestDistanceSqr) | ||
{ | ||
bestDistanceSqr = curDistanceSqr; | ||
bestId = waypoint.Id; | ||
} | ||
} | ||
return bestId; | ||
} | ||
|
||
public static bool IsTraversable(in Waypoint waypoint) => | ||
waypoint.Group == InvalidId || waypoint.WalkableIds.Length > 0 || waypoint.JumpableIds.Length > 0; | ||
|
||
public uint NearestTraversableId(Vector3 position) => NearestId(position, new TraversableFilter()); | ||
private readonly struct TraversableFilter : IWaypointFilter | ||
{ | ||
public bool IsValid(in Waypoint waypoint) => IsTraversable(waypoint); | ||
} | ||
|
||
public uint NearestJumpableId(Vector3 position) => NearestId(position, new JumpableFilter()); | ||
private readonly struct JumpableFilter : IWaypointFilter | ||
{ | ||
public bool IsValid(in Waypoint waypoint) => waypoint.JumpableIds.Length > 0; | ||
} | ||
|
||
public uint NearestIdOfGroup(Vector3 position, uint group) => NearestId(position, new GroupFilter(group)); | ||
public uint FurthestIdOfGroup(Vector3 position, uint group) => FurthestId(position, new GroupFilter(group)); | ||
private readonly struct GroupFilter(uint group) : IWaypointFilter | ||
{ | ||
public bool IsValid(in Waypoint waypoint) => waypoint.Group == group; | ||
} | ||
|
||
public uint FurthestIdOfCompatible(Vector3 position, uint group) => FurthestId(position, new CompatibleGroupFilter( | ||
wpSystem.CompatibleGroups.GetValueOrDefault(group, []), group)); | ||
private readonly struct CompatibleGroupFilter(uint[] compatibleGroups, uint group) : IWaypointFilter | ||
{ | ||
public bool IsValid(in Waypoint waypoint) => | ||
(waypoint.WalkableIds.Length > 0 || waypoint.JumpableIds.Length > 0) && | ||
(waypoint.Group == group || Array.IndexOf(compatibleGroups, waypoint.Group) >= 0); | ||
} | ||
|
||
public uint NearestInvisibleTo(Vector3 position, uint otherId) => NearestId(position, new NearestInvisibleFilter( | ||
new ArraySegment<bool>(isVisible, idToIndex[otherId] * WaypointCount, WaypointCount))); | ||
private readonly struct NearestInvisibleFilter(ArraySegment<bool> isVisible) : IWaypointFilter | ||
{ | ||
public bool IsValid(in Waypoint waypoint, int index) => | ||
!isVisible[index] && IsTraversable(waypoint); | ||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,10 @@ | ||
using System.Numerics; | ||
|
||
namespace zzre.game.messages; | ||
|
||
public readonly record struct GenerateAIPath( | ||
DefaultEcs.Entity ForEntity, | ||
uint CurrentWaypointId = uint.MaxValue, | ||
Vector3? CurrentPosition = null) | ||
{ | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,57 @@ | ||
using System; | ||
using System.Collections.Generic; | ||
using System.Linq; | ||
using System.Text; | ||
using System.Threading.Tasks; | ||
using DefaultEcs.System; | ||
|
||
namespace zzre.game.systems; | ||
|
||
public sealed partial class AIPath : ISystem<float> | ||
{ | ||
private const int DefaultPathLength = 128; | ||
|
||
private readonly DefaultEcs.World ecsWorld; | ||
private readonly IDisposable componentRemovedSubscription; | ||
private readonly IDisposable generateMessageSubscription; | ||
public bool IsEnabled { get; set; } | ||
|
||
public AIPath(ITagContainer diContainer) | ||
{ | ||
ecsWorld = diContainer.GetTag<DefaultEcs.World>(); | ||
componentRemovedSubscription = ecsWorld.SubscribeEntityComponentRemoved<components.AIPath>(HandleComponentRemoved); | ||
generateMessageSubscription = ecsWorld.Subscribe<messages.GenerateAIPath>(HandleGenerateAIPath); | ||
} | ||
|
||
public void Dispose() | ||
{ | ||
componentRemovedSubscription.Dispose(); | ||
generateMessageSubscription.Dispose(); | ||
} | ||
|
||
private void HandleComponentRemoved(in DefaultEcs.Entity entity, in components.AIPath value) | ||
{ | ||
value.WaypointIndices.Dispose(); | ||
} | ||
|
||
private void HandleGenerateAIPath(in messages.GenerateAIPath message) | ||
{ | ||
var optPath = message.ForEntity.TryGet<components.AIPath>(); | ||
if (!optPath.HasValue) | ||
{ | ||
message.ForEntity.Set<components.AIPath>(new() | ||
{ | ||
WaypointIndices = new(DefaultPathLength) | ||
}); | ||
optPath = new(ref message.ForEntity.Get<components.AIPath>()); | ||
} | ||
ref var path = ref optPath.Value; | ||
path.WaypointIndices.Clear(); | ||
|
||
|
||
} | ||
|
||
public void Update(float _) | ||
{ | ||
} | ||
} |