Skip to content

Commit

Permalink
Add initial PathFinder and assorted scribbles
Browse files Browse the repository at this point in the history
  • Loading branch information
Helco committed Nov 2, 2024
1 parent b2462ea commit f00e1a5
Show file tree
Hide file tree
Showing 4 changed files with 255 additions and 2 deletions.
183 changes: 183 additions & 0 deletions zzre/game/PathFinder.cs
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);
}
}
7 changes: 5 additions & 2 deletions zzre/game/components/ai/AIPath.cs
Original file line number Diff line number Diff line change
@@ -1,7 +1,10 @@
namespace zzre.game.components.ai;
using System.Numerics;

internal class AIPath
namespace zzre.game.components;

public struct AIPath
{
public PooledList<(uint Id, Vector3 Position)> WaypointIndices;
}

/*
Expand Down
10 changes: 10 additions & 0 deletions zzre/game/messages/duel/GenerateAIPath.cs
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)
{
}
57 changes: 57 additions & 0 deletions zzre/game/systems/duel/AIPath.cs
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 _)
{
}
}

0 comments on commit f00e1a5

Please sign in to comment.