Add MrGameEng.Pathfinding and MrGameEng.Collisions modules
CI / build-test (push) Failing after 1m5s
CI / build-test (push) Failing after 1m5s
Pathfinding (depends on Core only): GridPathfinder with A* (octile/
Manhattan heuristic), Dijkstra and BFS over a game-implemented
IPathGrid; 4/8 connectivity, diagonals never cut corners. FlowField +
FlowFieldBuilder (multi-source Dijkstra) give crowds O(1) steering per
agent per frame. All buffers are grid-sized once and invalidated by a
generation stamp - repeated queries allocate nothing and clear nothing.
Collisions (Graphics exception: Transform2D, RectF): Collider component
(circle/AABB, offset, two-way layer masks), CollisionWorld - a uniform
spatial hash on flat arrays rebuilt from scratch each tick (O(n) for
movers, zero alloc after warm-up, deterministic pair order), pair
collection, QueryAabb and closest-hit Raycast. scene.UseCollisions()
registers CollisionSystem after movement systems.
30 new tests (string-map mazes, cost weighting, corner cutting, flow
descent; pair/mask/query/raycast). Sample gains a PathfindingScene
('path' console command): click to set the goal, 250 agents follow the
flow field, the A* path is highlighted, colliding agents flash red.
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
Leonid Pershin
co-authored by
Claude Fable 5
Claude Fable 5
parent
b3415120c3
commit
a395e58458
@@ -0,0 +1,61 @@
|
||||
using Friflo.Engine.ECS;
|
||||
using Microsoft.Xna.Framework;
|
||||
|
||||
namespace MrGameEng.Collisions;
|
||||
|
||||
/// <summary>Shape of a <see cref="Collider"/>.</summary>
|
||||
public enum ColliderShape : byte
|
||||
{
|
||||
/// <summary>Circle of <see cref="Collider.Radius"/>.</summary>
|
||||
Circle,
|
||||
|
||||
/// <summary>Axis-aligned box of <see cref="Collider.HalfExtents"/>. Does not rotate with the entity.</summary>
|
||||
Box,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Collision shape component. Create via <see cref="Circle"/> or <see cref="Box"/> —
|
||||
/// the struct default has no size and collides with nothing.
|
||||
/// Positions come from <c>Transform2D</c>; <see cref="Offset"/> shifts the shape
|
||||
/// relative to it (entity scale and rotation are not applied to collider shapes).
|
||||
/// </summary>
|
||||
public struct Collider : IComponent
|
||||
{
|
||||
/// <summary>Shape kind.</summary>
|
||||
public ColliderShape Shape;
|
||||
|
||||
/// <summary>Circle radius in world units (<see cref="ColliderShape.Circle"/> only).</summary>
|
||||
public float Radius;
|
||||
|
||||
/// <summary>Half extents of the box (<see cref="ColliderShape.Box"/> only).</summary>
|
||||
public Vector2 HalfExtents;
|
||||
|
||||
/// <summary>Shape center offset from the entity's transform position.</summary>
|
||||
public Vector2 Offset;
|
||||
|
||||
/// <summary>Bit mask of layers this collider belongs to.</summary>
|
||||
public uint Layer;
|
||||
|
||||
/// <summary>Bit mask of layers this collider collides with. A pair is reported only when the masks agree both ways.</summary>
|
||||
public uint CollidesWith;
|
||||
|
||||
/// <summary>Creates a circle collider on layer 1 colliding with everything.</summary>
|
||||
public static Collider Circle(float radius, Vector2 offset = default) => new()
|
||||
{
|
||||
Shape = ColliderShape.Circle,
|
||||
Radius = radius,
|
||||
Offset = offset,
|
||||
Layer = 1,
|
||||
CollidesWith = uint.MaxValue,
|
||||
};
|
||||
|
||||
/// <summary>Creates a box collider on layer 1 colliding with everything.</summary>
|
||||
public static Collider Box(float width, float height, Vector2 offset = default) => new()
|
||||
{
|
||||
Shape = ColliderShape.Box,
|
||||
HalfExtents = new Vector2(width / 2f, height / 2f),
|
||||
Offset = offset,
|
||||
Layer = 1,
|
||||
CollidesWith = uint.MaxValue,
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
using Friflo.Engine.ECS;
|
||||
using Friflo.Engine.ECS.Systems;
|
||||
using MrGameEng.Core;
|
||||
using MrGameEng.Graphics;
|
||||
|
||||
namespace MrGameEng.Collisions;
|
||||
|
||||
/// <summary>
|
||||
/// Rebuilds the <see cref="CollisionWorld"/> from every entity that has both
|
||||
/// <c>Transform2D</c> and <see cref="Collider"/>. Register it <b>after</b> movement
|
||||
/// systems so pairs reflect this tick's final positions.
|
||||
/// </summary>
|
||||
public sealed class CollisionSystem : QuerySystem<Transform2D, Collider>
|
||||
{
|
||||
private readonly CollisionWorld _world;
|
||||
|
||||
/// <summary>Creates the system for <paramref name="world"/>.</summary>
|
||||
public CollisionSystem(CollisionWorld world) => _world = world;
|
||||
|
||||
/// <inheritdoc />
|
||||
protected override void OnUpdate()
|
||||
{
|
||||
_world.BeginRebuild();
|
||||
foreach (var (transforms, colliders, entities) in Query.Chunks)
|
||||
{
|
||||
var t = transforms.Span;
|
||||
var c = colliders.Span;
|
||||
for (var i = 0; i < t.Length; i++)
|
||||
{
|
||||
_world.Add(entities.EntityAt(i), in t[i], in c[i]);
|
||||
}
|
||||
}
|
||||
|
||||
_world.EndRebuild();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>Wires collision detection into a <see cref="Scene"/>.</summary>
|
||||
public static class SceneCollisionsExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// Returns the shared <see cref="CollisionWorld"/> service (created on first use with
|
||||
/// <paramref name="cellSize"/>) and adds <see cref="CollisionSystem"/> to this scene's
|
||||
/// update phase. Call from <c>OnLoad</c> <b>after</b> adding movement systems; read
|
||||
/// <see cref="CollisionWorld.Pairs"/> from systems registered later.
|
||||
/// </summary>
|
||||
public static CollisionWorld UseCollisions(this Scene scene, float cellSize = 64f)
|
||||
{
|
||||
var services = scene.Context.Services;
|
||||
var world = services.GetOrDefault<CollisionWorld>();
|
||||
if (world is null)
|
||||
{
|
||||
world = new CollisionWorld(cellSize);
|
||||
services.Add(world);
|
||||
}
|
||||
|
||||
scene.UpdateSystems.Add(new CollisionSystem(world));
|
||||
return world;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,388 @@
|
||||
using Friflo.Engine.ECS;
|
||||
using Microsoft.Xna.Framework;
|
||||
using MrGameEng.Graphics;
|
||||
|
||||
namespace MrGameEng.Collisions;
|
||||
|
||||
/// <summary>A pair of entities whose colliders overlap this frame.</summary>
|
||||
public readonly record struct CollisionPair(Entity A, Entity B);
|
||||
|
||||
/// <summary>Result of a <see cref="CollisionWorld.Raycast"/>.</summary>
|
||||
public readonly record struct RaycastHit(Entity Entity, Vector2 Point, float Fraction);
|
||||
|
||||
/// <summary>
|
||||
/// Broad + narrow phase collision detection over a uniform spatial hash, rebuilt from
|
||||
/// scratch every tick by <see cref="CollisionSystem"/> — O(n) for moving entities, flat
|
||||
/// arrays only, no allocations after warm-up. Overlapping pairs are collected into
|
||||
/// <see cref="Pairs"/> (deterministic order); ad-hoc area/ray queries are available to
|
||||
/// game systems at any point after the rebuild.
|
||||
/// </summary>
|
||||
public sealed class CollisionWorld
|
||||
{
|
||||
private struct Entry
|
||||
{
|
||||
public Entity Entity;
|
||||
public Vector2 Center;
|
||||
public RectF Aabb;
|
||||
public float Radius;
|
||||
public Vector2 HalfExtents;
|
||||
public uint Layer;
|
||||
public uint CollidesWith;
|
||||
public ColliderShape Shape;
|
||||
}
|
||||
|
||||
private readonly float _cellSize;
|
||||
private Entry[] _entries = new Entry[256];
|
||||
private int _count;
|
||||
|
||||
// Spatial hash: головы бакетов + связные списки вставок (по индексу записи на ячейку).
|
||||
private int[] _bucketHeads = new int[512];
|
||||
private int[] _cellNext = new int[1024];
|
||||
private int[] _cellEntry = new int[1024];
|
||||
private int _cellCount;
|
||||
|
||||
private int[] _testedStamp = new int[256];
|
||||
private CollisionPair[] _pairs = new CollisionPair[256];
|
||||
private int _pairCount;
|
||||
|
||||
/// <summary>Creates a world. <paramref name="cellSize"/> should match typical collider size.</summary>
|
||||
public CollisionWorld(float cellSize = 64f)
|
||||
{
|
||||
if (cellSize <= 0f)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(cellSize));
|
||||
}
|
||||
|
||||
_cellSize = cellSize;
|
||||
}
|
||||
|
||||
/// <summary>Pairs found by the last rebuild.</summary>
|
||||
public ReadOnlySpan<CollisionPair> Pairs => _pairs.AsSpan(0, _pairCount);
|
||||
|
||||
/// <summary>Colliders registered in the last rebuild.</summary>
|
||||
public int Count => _count;
|
||||
|
||||
/// <summary>Starts a rebuild. Called by <see cref="CollisionSystem"/> once per tick.</summary>
|
||||
public void BeginRebuild()
|
||||
{
|
||||
_count = 0;
|
||||
_cellCount = 0;
|
||||
_pairCount = 0;
|
||||
}
|
||||
|
||||
/// <summary>Registers one collider. Order of registration defines pair order (keep it deterministic).</summary>
|
||||
public void Add(Entity entity, in Transform2D transform, in Collider collider)
|
||||
{
|
||||
if (_count == _entries.Length)
|
||||
{
|
||||
Array.Resize(ref _entries, _entries.Length * 2);
|
||||
Array.Resize(ref _testedStamp, _entries.Length);
|
||||
}
|
||||
|
||||
var center = transform.Position + collider.Offset;
|
||||
var half = collider.Shape == ColliderShape.Circle
|
||||
? new Vector2(collider.Radius)
|
||||
: collider.HalfExtents;
|
||||
|
||||
_entries[_count++] = new Entry
|
||||
{
|
||||
Entity = entity,
|
||||
Center = center,
|
||||
Aabb = new RectF(center.X - half.X, center.Y - half.Y, half.X * 2f, half.Y * 2f),
|
||||
Radius = collider.Radius,
|
||||
HalfExtents = collider.HalfExtents,
|
||||
Layer = collider.Layer,
|
||||
CollidesWith = collider.CollidesWith,
|
||||
Shape = collider.Shape,
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>Builds the hash and collects all overlapping pairs.</summary>
|
||||
public void EndRebuild()
|
||||
{
|
||||
BuildHash();
|
||||
CollectPairs();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Writes entities whose colliders overlap <paramref name="area"/> into
|
||||
/// <paramref name="results"/>; returns the count (truncated to the span length).
|
||||
/// </summary>
|
||||
public int QueryAabb(in RectF area, Span<Entity> results)
|
||||
{
|
||||
var found = 0;
|
||||
var minX = CellOf(area.Left);
|
||||
var maxX = CellOf(area.Right);
|
||||
var minY = CellOf(area.Top);
|
||||
var maxY = CellOf(area.Bottom);
|
||||
var stamp = -1; // запросы используют отрицательные штампы, чтобы не портить пары
|
||||
|
||||
for (var cy = minY; cy <= maxY; cy++)
|
||||
{
|
||||
for (var cx = minX; cx <= maxX; cx++)
|
||||
{
|
||||
for (var i = _bucketHeads[Bucket(cx, cy)]; i >= 0; i = _cellNext[i])
|
||||
{
|
||||
var entryIndex = _cellEntry[i];
|
||||
if (_testedStamp[entryIndex] == stamp)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
_testedStamp[entryIndex] = stamp;
|
||||
if (_entries[entryIndex].Aabb.Intersects(area))
|
||||
{
|
||||
if (found == results.Length)
|
||||
{
|
||||
return found;
|
||||
}
|
||||
|
||||
results[found++] = _entries[entryIndex].Entity;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ResetQueryStamps();
|
||||
return found;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Casts a segment and returns the closest hit among colliders whose
|
||||
/// <see cref="Collider.Layer"/> intersects <paramref name="mask"/>.
|
||||
/// </summary>
|
||||
public bool Raycast(Vector2 from, Vector2 to, out RaycastHit hit, uint mask = uint.MaxValue)
|
||||
{
|
||||
hit = default;
|
||||
var bestFraction = float.MaxValue;
|
||||
|
||||
for (var i = 0; i < _count; i++)
|
||||
{
|
||||
ref readonly var entry = ref _entries[i];
|
||||
if ((entry.Layer & mask) == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
float fraction;
|
||||
var found = entry.Shape == ColliderShape.Circle
|
||||
? RaySegmentCircle(from, to, entry.Center, entry.Radius, out fraction)
|
||||
: RaySegmentAabb(from, to, entry.Aabb, out fraction);
|
||||
|
||||
if (found && fraction < bestFraction)
|
||||
{
|
||||
bestFraction = fraction;
|
||||
hit = new RaycastHit(entry.Entity, Vector2.Lerp(from, to, fraction), fraction);
|
||||
}
|
||||
}
|
||||
|
||||
return bestFraction <= 1f;
|
||||
}
|
||||
|
||||
private void BuildHash()
|
||||
{
|
||||
var buckets = _bucketHeads.Length;
|
||||
while (buckets < _count * 2)
|
||||
{
|
||||
buckets *= 2;
|
||||
}
|
||||
|
||||
if (buckets != _bucketHeads.Length)
|
||||
{
|
||||
_bucketHeads = new int[buckets];
|
||||
}
|
||||
|
||||
Array.Fill(_bucketHeads, -1);
|
||||
|
||||
for (var i = 0; i < _count; i++)
|
||||
{
|
||||
ref readonly var aabb = ref _entries[i].Aabb;
|
||||
var minX = CellOf(aabb.Left);
|
||||
var maxX = CellOf(aabb.Right);
|
||||
var minY = CellOf(aabb.Top);
|
||||
var maxY = CellOf(aabb.Bottom);
|
||||
for (var cy = minY; cy <= maxY; cy++)
|
||||
{
|
||||
for (var cx = minX; cx <= maxX; cx++)
|
||||
{
|
||||
if (_cellCount == _cellNext.Length)
|
||||
{
|
||||
Array.Resize(ref _cellNext, _cellNext.Length * 2);
|
||||
Array.Resize(ref _cellEntry, _cellEntry.Length * 2);
|
||||
}
|
||||
|
||||
var bucket = Bucket(cx, cy);
|
||||
_cellEntry[_cellCount] = i;
|
||||
_cellNext[_cellCount] = _bucketHeads[bucket];
|
||||
_bucketHeads[bucket] = _cellCount;
|
||||
_cellCount++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (var i = 0; i < _count; i++)
|
||||
{
|
||||
_testedStamp[i] = int.MinValue;
|
||||
}
|
||||
}
|
||||
|
||||
private void CollectPairs()
|
||||
{
|
||||
for (var i = 0; i < _count; i++)
|
||||
{
|
||||
ref readonly var a = ref _entries[i];
|
||||
var minX = CellOf(a.Aabb.Left);
|
||||
var maxX = CellOf(a.Aabb.Right);
|
||||
var minY = CellOf(a.Aabb.Top);
|
||||
var maxY = CellOf(a.Aabb.Bottom);
|
||||
|
||||
for (var cy = minY; cy <= maxY; cy++)
|
||||
{
|
||||
for (var cx = minX; cx <= maxX; cx++)
|
||||
{
|
||||
for (var c = _bucketHeads[Bucket(cx, cy)]; c >= 0; c = _cellNext[c])
|
||||
{
|
||||
var j = _cellEntry[c];
|
||||
if (j <= i || _testedStamp[j] == i)
|
||||
{
|
||||
continue; // только пары (i, j>i), каждая один раз
|
||||
}
|
||||
|
||||
_testedStamp[j] = i;
|
||||
ref readonly var b = ref _entries[j];
|
||||
if ((a.Layer & b.CollidesWith) == 0 || (b.Layer & a.CollidesWith) == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (Overlaps(in a, in b))
|
||||
{
|
||||
if (_pairCount == _pairs.Length)
|
||||
{
|
||||
Array.Resize(ref _pairs, _pairs.Length * 2);
|
||||
}
|
||||
|
||||
_pairs[_pairCount++] = new CollisionPair(a.Entity, b.Entity);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static bool Overlaps(in Entry a, in Entry b)
|
||||
{
|
||||
if (a.Shape == ColliderShape.Circle && b.Shape == ColliderShape.Circle)
|
||||
{
|
||||
var sum = a.Radius + b.Radius;
|
||||
return Vector2.DistanceSquared(a.Center, b.Center) <= sum * sum;
|
||||
}
|
||||
|
||||
if (a.Shape == ColliderShape.Box && b.Shape == ColliderShape.Box)
|
||||
{
|
||||
return a.Aabb.Intersects(b.Aabb);
|
||||
}
|
||||
|
||||
// circle vs box
|
||||
ref readonly var circle = ref a.Shape == ColliderShape.Circle ? ref a : ref b;
|
||||
ref readonly var box = ref a.Shape == ColliderShape.Circle ? ref b : ref a;
|
||||
var nearest = new Vector2(
|
||||
Math.Clamp(circle.Center.X, box.Aabb.Left, box.Aabb.Right),
|
||||
Math.Clamp(circle.Center.Y, box.Aabb.Top, box.Aabb.Bottom));
|
||||
return Vector2.DistanceSquared(circle.Center, nearest) <= circle.Radius * circle.Radius;
|
||||
}
|
||||
|
||||
private static bool RaySegmentCircle(Vector2 from, Vector2 to, Vector2 center, float radius, out float fraction)
|
||||
{
|
||||
fraction = 0f;
|
||||
var d = to - from;
|
||||
var f = from - center;
|
||||
var a = Vector2.Dot(d, d);
|
||||
if (a <= float.Epsilon)
|
||||
{
|
||||
return f.LengthSquared() <= radius * radius;
|
||||
}
|
||||
|
||||
var b = 2f * Vector2.Dot(f, d);
|
||||
var c = Vector2.Dot(f, f) - radius * radius;
|
||||
var discriminant = b * b - 4f * a * c;
|
||||
if (discriminant < 0f)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var sqrt = MathF.Sqrt(discriminant);
|
||||
var t = (-b - sqrt) / (2f * a);
|
||||
if (t < 0f)
|
||||
{
|
||||
t = (-b + sqrt) / (2f * a); // старт внутри круга
|
||||
}
|
||||
|
||||
if (t < 0f || t > 1f)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
fraction = t;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool RaySegmentAabb(Vector2 from, Vector2 to, in RectF aabb, out float fraction)
|
||||
{
|
||||
fraction = 0f;
|
||||
var d = to - from;
|
||||
var tMin = 0f;
|
||||
var tMax = 1f;
|
||||
|
||||
for (var axis = 0; axis < 2; axis++)
|
||||
{
|
||||
var origin = axis == 0 ? from.X : from.Y;
|
||||
var direction = axis == 0 ? d.X : d.Y;
|
||||
var min = axis == 0 ? aabb.Left : aabb.Top;
|
||||
var max = axis == 0 ? aabb.Right : aabb.Bottom;
|
||||
|
||||
if (Math.Abs(direction) < float.Epsilon)
|
||||
{
|
||||
if (origin < min || origin > max)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
var t1 = (min - origin) / direction;
|
||||
var t2 = (max - origin) / direction;
|
||||
if (t1 > t2)
|
||||
{
|
||||
(t1, t2) = (t2, t1);
|
||||
}
|
||||
|
||||
tMin = Math.Max(tMin, t1);
|
||||
tMax = Math.Min(tMax, t2);
|
||||
if (tMin > tMax)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fraction = tMin;
|
||||
return true;
|
||||
}
|
||||
|
||||
private void ResetQueryStamps()
|
||||
{
|
||||
for (var i = 0; i < _count; i++)
|
||||
{
|
||||
if (_testedStamp[i] < 0)
|
||||
{
|
||||
_testedStamp[i] = int.MinValue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private int CellOf(float coordinate) => (int)MathF.Floor(coordinate / _cellSize);
|
||||
|
||||
private int Bucket(int cellX, int cellY) =>
|
||||
(int)(((uint)(cellX * 73856093 ^ cellY * 19349663)) & (uint)(_bucketHeads.Length - 1));
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\MrGameEng.Core\MrGameEng.Core.csproj" />
|
||||
<ProjectReference Include="..\MrGameEng.Graphics\MrGameEng.Graphics.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,244 @@
|
||||
using Microsoft.Xna.Framework;
|
||||
|
||||
namespace MrGameEng.Pathfinding;
|
||||
|
||||
/// <summary>
|
||||
/// A flow field: per-cell distance to the nearest goal and a normalized direction to follow.
|
||||
/// Built once per goal change by <see cref="FlowFieldBuilder"/>, then any number of agents
|
||||
/// steer by an O(1) lookup per frame — the tool of choice for crowds heading to shared targets.
|
||||
/// </summary>
|
||||
public sealed class FlowField
|
||||
{
|
||||
/// <summary>Grid width in cells.</summary>
|
||||
public int Width { get; private set; }
|
||||
|
||||
/// <summary>Grid height in cells.</summary>
|
||||
public int Height { get; private set; }
|
||||
|
||||
internal float[] Distances = [];
|
||||
internal Vector2[] Directions = [];
|
||||
|
||||
/// <summary>Cost-weighted distance to the nearest goal; <see cref="float.PositiveInfinity"/> when unreachable.</summary>
|
||||
public float DistanceAt(int x, int y) => Distances[y * Width + x];
|
||||
|
||||
/// <summary>Normalized direction toward the nearest goal; <see cref="Vector2.Zero"/> at goals and unreachable cells.</summary>
|
||||
public Vector2 DirectionAt(int x, int y) => Directions[y * Width + x];
|
||||
|
||||
/// <summary>True when a path to a goal exists from this cell.</summary>
|
||||
public bool IsReachable(int x, int y) => !float.IsPositiveInfinity(Distances[y * Width + x]);
|
||||
|
||||
internal void EnsureSize(int width, int height)
|
||||
{
|
||||
Width = width;
|
||||
Height = height;
|
||||
var size = width * height;
|
||||
if (Distances.Length < size)
|
||||
{
|
||||
Distances = new float[size];
|
||||
Directions = new Vector2[size];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Builds <see cref="FlowField"/>s with a multi-source Dijkstra over an <see cref="IPathGrid"/>.
|
||||
/// Buffers are reused between builds (no allocations after warm-up). One instance per system.
|
||||
/// </summary>
|
||||
public sealed class FlowFieldBuilder
|
||||
{
|
||||
private static readonly int[] OffsetX = [1, -1, 0, 0, 1, 1, -1, -1];
|
||||
private static readonly int[] OffsetY = [0, 0, 1, -1, 1, -1, 1, -1];
|
||||
private const float DiagonalCost = 1.4142135f;
|
||||
|
||||
private readonly IPathGrid _grid;
|
||||
private readonly GridConnectivity _connectivity;
|
||||
private readonly int _width;
|
||||
private readonly int _height;
|
||||
private readonly int[] _closedStamp;
|
||||
private int[] _heapNodes;
|
||||
private float[] _heapPriorities;
|
||||
private int _generation;
|
||||
private int _heapCount;
|
||||
|
||||
/// <summary>Creates a builder bound to <paramref name="grid"/>.</summary>
|
||||
public FlowFieldBuilder(IPathGrid grid, GridConnectivity connectivity = GridConnectivity.Eight)
|
||||
{
|
||||
_grid = grid;
|
||||
_connectivity = connectivity;
|
||||
_width = grid.Width;
|
||||
_height = grid.Height;
|
||||
var size = _width * _height;
|
||||
_closedStamp = new int[size];
|
||||
_heapNodes = new int[size + 1];
|
||||
_heapPriorities = new float[size + 1];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Fills <paramref name="field"/> with distances and directions toward the nearest of
|
||||
/// <paramref name="goals"/>. Impassable goals are ignored; with no valid goal the whole
|
||||
/// field is unreachable.
|
||||
/// </summary>
|
||||
public void Build(ReadOnlySpan<Point> goals, FlowField field)
|
||||
{
|
||||
field.EnsureSize(_width, _height);
|
||||
var distances = field.Distances;
|
||||
var directions = field.Directions;
|
||||
Array.Fill(distances, float.PositiveInfinity, 0, _width * _height);
|
||||
|
||||
_generation++;
|
||||
_heapCount = 0;
|
||||
|
||||
foreach (var goal in goals)
|
||||
{
|
||||
if (goal.X >= 0 && goal.X < _width && goal.Y >= 0 && goal.Y < _height &&
|
||||
_grid.IsPassable(goal.X, goal.Y))
|
||||
{
|
||||
var index = goal.Y * _width + goal.X;
|
||||
distances[index] = 0f;
|
||||
HeapPush(index, 0f);
|
||||
}
|
||||
}
|
||||
|
||||
var directionCount = (int)_connectivity;
|
||||
while (_heapCount > 0)
|
||||
{
|
||||
var current = HeapPop();
|
||||
if (_closedStamp[current] == _generation)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
_closedStamp[current] = _generation;
|
||||
var cx = current % _width;
|
||||
var cy = current / _width;
|
||||
|
||||
for (var d = 0; d < directionCount; d++)
|
||||
{
|
||||
var nx = cx + OffsetX[d];
|
||||
var ny = cy + OffsetY[d];
|
||||
if (!Walkable(cx, cy, nx, ny, d))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var neighbor = ny * _width + nx;
|
||||
var tentative = distances[current] + (d < 4 ? 1f : DiagonalCost) * _grid.Cost(nx, ny);
|
||||
if (tentative < distances[neighbor])
|
||||
{
|
||||
distances[neighbor] = tentative;
|
||||
HeapPush(neighbor, tentative);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Направление — к соседу с минимальной дистанцией (с учётом запрета срезать углы).
|
||||
for (var y = 0; y < _height; y++)
|
||||
{
|
||||
for (var x = 0; x < _width; x++)
|
||||
{
|
||||
var index = y * _width + x;
|
||||
directions[index] = Vector2.Zero;
|
||||
if (float.IsPositiveInfinity(distances[index]) || distances[index] == 0f)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var best = distances[index];
|
||||
var bestDx = 0;
|
||||
var bestDy = 0;
|
||||
for (var d = 0; d < directionCount; d++)
|
||||
{
|
||||
var nx = x + OffsetX[d];
|
||||
var ny = y + OffsetY[d];
|
||||
if (!Walkable(x, y, nx, ny, d))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var distance = distances[ny * _width + nx];
|
||||
if (distance < best)
|
||||
{
|
||||
best = distance;
|
||||
bestDx = OffsetX[d];
|
||||
bestDy = OffsetY[d];
|
||||
}
|
||||
}
|
||||
|
||||
if (bestDx != 0 || bestDy != 0)
|
||||
{
|
||||
directions[index] = Vector2.Normalize(new Vector2(bestDx, bestDy));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private bool Walkable(int fromX, int fromY, int toX, int toY, int direction)
|
||||
{
|
||||
if (toX < 0 || toX >= _width || toY < 0 || toY >= _height || !_grid.IsPassable(toX, toY))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return direction < 4 || (_grid.IsPassable(toX, fromY) && _grid.IsPassable(fromX, toY));
|
||||
}
|
||||
|
||||
private void HeapPush(int node, float priority)
|
||||
{
|
||||
if (_heapCount + 1 == _heapNodes.Length)
|
||||
{
|
||||
Array.Resize(ref _heapNodes, _heapNodes.Length * 2);
|
||||
Array.Resize(ref _heapPriorities, _heapPriorities.Length * 2);
|
||||
}
|
||||
|
||||
var i = ++_heapCount;
|
||||
while (i > 1 && _heapPriorities[i >> 1] > priority)
|
||||
{
|
||||
_heapNodes[i] = _heapNodes[i >> 1];
|
||||
_heapPriorities[i] = _heapPriorities[i >> 1];
|
||||
i >>= 1;
|
||||
}
|
||||
|
||||
_heapNodes[i] = node;
|
||||
_heapPriorities[i] = priority;
|
||||
}
|
||||
|
||||
private int HeapPop()
|
||||
{
|
||||
var top = _heapNodes[1];
|
||||
var lastNode = _heapNodes[_heapCount];
|
||||
var lastPriority = _heapPriorities[_heapCount];
|
||||
_heapCount--;
|
||||
|
||||
var i = 1;
|
||||
while (true)
|
||||
{
|
||||
var child = i << 1;
|
||||
if (child > _heapCount)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (child < _heapCount && _heapPriorities[child + 1] < _heapPriorities[child])
|
||||
{
|
||||
child++;
|
||||
}
|
||||
|
||||
if (_heapPriorities[child] >= lastPriority)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
_heapNodes[i] = _heapNodes[child];
|
||||
_heapPriorities[i] = _heapPriorities[child];
|
||||
i = child;
|
||||
}
|
||||
|
||||
if (_heapCount > 0)
|
||||
{
|
||||
_heapNodes[i] = lastNode;
|
||||
_heapPriorities[i] = lastPriority;
|
||||
}
|
||||
|
||||
return top;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,301 @@
|
||||
using Microsoft.Xna.Framework;
|
||||
|
||||
namespace MrGameEng.Pathfinding;
|
||||
|
||||
/// <summary>Algorithm used by <see cref="GridPathfinder.FindPath"/>.</summary>
|
||||
public enum PathAlgorithm
|
||||
{
|
||||
/// <summary>Best general choice: cost-aware, goal-directed (octile/Manhattan heuristic).</summary>
|
||||
AStar,
|
||||
|
||||
/// <summary>Cost-aware without a heuristic. Slower than A*, useful as a reference.</summary>
|
||||
Dijkstra,
|
||||
|
||||
/// <summary>Fastest for uniform-cost grids; ignores <see cref="IPathGrid.Cost"/>.</summary>
|
||||
BreadthFirst,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Grid pathfinder with A*, Dijkstra and BFS over an <see cref="IPathGrid"/>.
|
||||
/// All working buffers are sized to the grid once and invalidated by a generation stamp,
|
||||
/// so repeated queries allocate nothing and never clear arrays. One instance per system
|
||||
/// (not thread-safe); results are deterministic for identical inputs.
|
||||
/// </summary>
|
||||
public sealed class GridPathfinder
|
||||
{
|
||||
private static readonly int[] OffsetX = [1, -1, 0, 0, 1, 1, -1, -1];
|
||||
private static readonly int[] OffsetY = [0, 0, 1, -1, 1, -1, 1, -1];
|
||||
private const float DiagonalCost = 1.4142135f;
|
||||
|
||||
private readonly IPathGrid _grid;
|
||||
private readonly GridConnectivity _connectivity;
|
||||
private readonly int _width;
|
||||
private readonly int _height;
|
||||
|
||||
private readonly float[] _gScore;
|
||||
private readonly int[] _cameFrom;
|
||||
private readonly int[] _openStamp;
|
||||
private readonly int[] _closedStamp;
|
||||
private int[] _heapNodes;
|
||||
private float[] _heapPriorities;
|
||||
private readonly int[] _bfsQueue;
|
||||
private int _generation;
|
||||
private int _heapCount;
|
||||
|
||||
/// <summary>Creates a pathfinder bound to <paramref name="grid"/>.</summary>
|
||||
public GridPathfinder(IPathGrid grid, GridConnectivity connectivity = GridConnectivity.Eight)
|
||||
{
|
||||
_grid = grid;
|
||||
_connectivity = connectivity;
|
||||
_width = grid.Width;
|
||||
_height = grid.Height;
|
||||
var size = _width * _height;
|
||||
_gScore = new float[size];
|
||||
_cameFrom = new int[size];
|
||||
_openStamp = new int[size];
|
||||
_closedStamp = new int[size];
|
||||
_heapNodes = new int[size + 1];
|
||||
_heapPriorities = new float[size + 1];
|
||||
_bfsQueue = new int[size];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Finds a path from <paramref name="start"/> to <paramref name="goal"/> (both inclusive)
|
||||
/// and writes it into <paramref name="path"/>. Returns false when no path exists;
|
||||
/// the list is cleared either way.
|
||||
/// </summary>
|
||||
public bool FindPath(Point start, Point goal, List<Point> path, PathAlgorithm algorithm = PathAlgorithm.AStar)
|
||||
{
|
||||
path.Clear();
|
||||
if (!InBounds(start) || !InBounds(goal) ||
|
||||
!_grid.IsPassable(start.X, start.Y) || !_grid.IsPassable(goal.X, goal.Y))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (start == goal)
|
||||
{
|
||||
path.Add(start);
|
||||
return true;
|
||||
}
|
||||
|
||||
return algorithm == PathAlgorithm.BreadthFirst
|
||||
? BreadthFirst(start, goal, path)
|
||||
: WeightedSearch(start, goal, path, useHeuristic: algorithm == PathAlgorithm.AStar);
|
||||
}
|
||||
|
||||
private bool WeightedSearch(Point start, Point goal, List<Point> path, bool useHeuristic)
|
||||
{
|
||||
_generation++;
|
||||
_heapCount = 0;
|
||||
|
||||
var startIndex = Index(start.X, start.Y);
|
||||
var goalIndex = Index(goal.X, goal.Y);
|
||||
_gScore[startIndex] = 0f;
|
||||
_cameFrom[startIndex] = -1;
|
||||
_openStamp[startIndex] = _generation;
|
||||
HeapPush(startIndex, useHeuristic ? Heuristic(start.X, start.Y, goal) : 0f);
|
||||
|
||||
var directions = (int)_connectivity;
|
||||
while (_heapCount > 0)
|
||||
{
|
||||
var current = HeapPop();
|
||||
if (current == goalIndex)
|
||||
{
|
||||
Reconstruct(goalIndex, path);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (_closedStamp[current] == _generation)
|
||||
{
|
||||
continue; // устаревшая запись кучи
|
||||
}
|
||||
|
||||
_closedStamp[current] = _generation;
|
||||
var cx = current % _width;
|
||||
var cy = current / _width;
|
||||
|
||||
for (var d = 0; d < directions; d++)
|
||||
{
|
||||
var nx = cx + OffsetX[d];
|
||||
var ny = cy + OffsetY[d];
|
||||
if (!Walkable(cx, cy, nx, ny, d))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var neighbor = Index(nx, ny);
|
||||
if (_closedStamp[neighbor] == _generation)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var stepCost = (d < 4 ? 1f : DiagonalCost) * _grid.Cost(nx, ny);
|
||||
var tentative = _gScore[current] + stepCost;
|
||||
if (_openStamp[neighbor] == _generation && tentative >= _gScore[neighbor])
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
_gScore[neighbor] = tentative;
|
||||
_cameFrom[neighbor] = current;
|
||||
_openStamp[neighbor] = _generation;
|
||||
HeapPush(neighbor, tentative + (useHeuristic ? Heuristic(nx, ny, goal) : 0f));
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private bool BreadthFirst(Point start, Point goal, List<Point> path)
|
||||
{
|
||||
_generation++;
|
||||
var head = 0;
|
||||
var tail = 0;
|
||||
|
||||
var startIndex = Index(start.X, start.Y);
|
||||
var goalIndex = Index(goal.X, goal.Y);
|
||||
_cameFrom[startIndex] = -1;
|
||||
_openStamp[startIndex] = _generation;
|
||||
_bfsQueue[tail++] = startIndex;
|
||||
|
||||
var directions = (int)_connectivity;
|
||||
while (head < tail)
|
||||
{
|
||||
var current = _bfsQueue[head++];
|
||||
if (current == goalIndex)
|
||||
{
|
||||
Reconstruct(goalIndex, path);
|
||||
return true;
|
||||
}
|
||||
|
||||
var cx = current % _width;
|
||||
var cy = current / _width;
|
||||
for (var d = 0; d < directions; d++)
|
||||
{
|
||||
var nx = cx + OffsetX[d];
|
||||
var ny = cy + OffsetY[d];
|
||||
if (!Walkable(cx, cy, nx, ny, d))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var neighbor = Index(nx, ny);
|
||||
if (_openStamp[neighbor] == _generation)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
_openStamp[neighbor] = _generation;
|
||||
_cameFrom[neighbor] = current;
|
||||
_bfsQueue[tail++] = neighbor;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>True when the move is in bounds, passable and does not cut a corner.</summary>
|
||||
private bool Walkable(int fromX, int fromY, int toX, int toY, int direction)
|
||||
{
|
||||
if (toX < 0 || toX >= _width || toY < 0 || toY >= _height || !_grid.IsPassable(toX, toY))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (direction >= 4)
|
||||
{
|
||||
// Диагональ разрешена только если обе ортогональные клетки проходимы.
|
||||
if (!_grid.IsPassable(toX, fromY) || !_grid.IsPassable(fromX, toY))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private float Heuristic(int x, int y, Point goal)
|
||||
{
|
||||
var dx = Math.Abs(x - goal.X);
|
||||
var dy = Math.Abs(y - goal.Y);
|
||||
return _connectivity == GridConnectivity.Four
|
||||
? dx + dy
|
||||
: Math.Max(dx, dy) + (DiagonalCost - 1f) * Math.Min(dx, dy);
|
||||
}
|
||||
|
||||
private void Reconstruct(int goalIndex, List<Point> path)
|
||||
{
|
||||
for (var index = goalIndex; index >= 0; index = _cameFrom[index])
|
||||
{
|
||||
path.Add(new Point(index % _width, index / _width));
|
||||
}
|
||||
|
||||
path.Reverse();
|
||||
}
|
||||
|
||||
private bool InBounds(Point p) => p.X >= 0 && p.X < _width && p.Y >= 0 && p.Y < _height;
|
||||
|
||||
private int Index(int x, int y) => y * _width + x;
|
||||
|
||||
private void HeapPush(int node, float priority)
|
||||
{
|
||||
// Ленивая вставка кладёт узел повторно при улучшении пути — куче нужен запас.
|
||||
if (_heapCount + 1 == _heapNodes.Length)
|
||||
{
|
||||
Array.Resize(ref _heapNodes, _heapNodes.Length * 2);
|
||||
Array.Resize(ref _heapPriorities, _heapPriorities.Length * 2);
|
||||
}
|
||||
|
||||
var i = ++_heapCount;
|
||||
while (i > 1 && _heapPriorities[i >> 1] > priority)
|
||||
{
|
||||
_heapNodes[i] = _heapNodes[i >> 1];
|
||||
_heapPriorities[i] = _heapPriorities[i >> 1];
|
||||
i >>= 1;
|
||||
}
|
||||
|
||||
_heapNodes[i] = node;
|
||||
_heapPriorities[i] = priority;
|
||||
}
|
||||
|
||||
private int HeapPop()
|
||||
{
|
||||
var top = _heapNodes[1];
|
||||
var lastNode = _heapNodes[_heapCount];
|
||||
var lastPriority = _heapPriorities[_heapCount];
|
||||
_heapCount--;
|
||||
|
||||
var i = 1;
|
||||
while (true)
|
||||
{
|
||||
var child = i << 1;
|
||||
if (child > _heapCount)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (child < _heapCount && _heapPriorities[child + 1] < _heapPriorities[child])
|
||||
{
|
||||
child++;
|
||||
}
|
||||
|
||||
if (_heapPriorities[child] >= lastPriority)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
_heapNodes[i] = _heapNodes[child];
|
||||
_heapPriorities[i] = _heapPriorities[child];
|
||||
i = child;
|
||||
}
|
||||
|
||||
if (_heapCount > 0)
|
||||
{
|
||||
_heapNodes[i] = lastNode;
|
||||
_heapPriorities[i] = lastPriority;
|
||||
}
|
||||
|
||||
return top;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
namespace MrGameEng.Pathfinding;
|
||||
|
||||
/// <summary>Neighbor connectivity of a grid.</summary>
|
||||
public enum GridConnectivity
|
||||
{
|
||||
/// <summary>Orthogonal moves only.</summary>
|
||||
Four = 4,
|
||||
|
||||
/// <summary>Orthogonal and diagonal moves. Diagonals never cut corners.</summary>
|
||||
Eight = 8,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// A grid the pathfinding algorithms operate on. The game implements this over its own
|
||||
/// world representation (terrain cells, a <c>TileGrid</c>, …) — the pathfinding module
|
||||
/// never owns world data.
|
||||
/// </summary>
|
||||
public interface IPathGrid
|
||||
{
|
||||
/// <summary>Grid width in cells.</summary>
|
||||
int Width { get; }
|
||||
|
||||
/// <summary>Grid height in cells.</summary>
|
||||
int Height { get; }
|
||||
|
||||
/// <summary>True when the cell can be entered. Out-of-range cells are never queried.</summary>
|
||||
bool IsPassable(int x, int y);
|
||||
|
||||
/// <summary>
|
||||
/// Cost multiplier for entering the cell, <b>must be ≥ 1</b> (1 = normal terrain,
|
||||
/// 3 = swamp three times slower, …). Used by A* and Dijkstra; ignored by BFS.
|
||||
/// </summary>
|
||||
float Cost(int x, int y);
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\MrGameEng.Core\MrGameEng.Core.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
Reference in New Issue
Block a user