Skip to content

Instantly share code, notes, and snippets.

@1ceOcean

1ceOcean/bench.cs

Created July 9, 2026 08:48
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save 1ceOcean/95a6a5727cc2284d0119b02af3cdff3f to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save 1ceOcean/95a6a5727cc2284d0119b02af3cdff3f to your computer and use it in GitHub Desktop.
Download ZIP
use `dotnet run -c Release --project benchmarks/Hshg.SingleFileBench -- --dimension 2` run c like bench
Raw
bench.cs
using System.Diagnostics;
using System.Globalization;
using System.Numerics;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
Thread.CurrentThread.CurrentCulture = CultureInfo.InvariantCulture;
BenchOptions options = BenchOptions.Parse(args);
if (options.Dimension == 3)
{
ConsoleBench3D.Run(options);
}
else
{
ConsoleBench2D.Run(options);
}
internal readonly record struct BenchOptions(int Dimension, bool Lite, int? Seed, int Samples, int Windows)
{
public static BenchOptions Parse(string[] args)
{
int dimension = 2;
bool lite = false;
int? seed = null;
int samples = 200;
int windows = 0;
for (int i = 0; i < args.Length; i++)
{
switch (args[i])
{
case "--dimension":
dimension = int.Parse(args[++i], CultureInfo.InvariantCulture);
break;
case "--lite":
lite = true;
break;
case "--seed":
seed = int.Parse(args[++i], CultureInfo.InvariantCulture);
break;
case "--samples":
samples = int.Parse(args[++i], CultureInfo.InvariantCulture);
break;
case "--windows":
windows = int.Parse(args[++i], CultureInfo.InvariantCulture);
break;
case "--help":
case "-h":
Console.WriteLine("Usage: dotnet run -c Release --project benchmarks/Hshg.SingleFileBench -- [--dimension 2|3] [--lite] [--seed N] [--samples 200] [--windows N]");
Environment.Exit(0);
break;
}
}
if (dimension is not 2 and not 3)
{
throw new ArgumentOutOfRangeException(nameof(args), "--dimension must be 2 or 3.");
}
if (samples <= 0)
{
throw new ArgumentOutOfRangeException(nameof(args), "--samples must be positive.");
}
return new BenchOptions(dimension, lite, seed, samples, windows);
}
}
internal static class ConsoleBench2D
{
public static void Run(BenchOptions options)
{
int agents = options.Lite ? 100_000 : 500_000;
int side = options.Lite ? 1024 : 2048;
var sim = new Scenario2D(agents, side, 16, 7, options.Seed ?? Environment.TickCount);
sim.InsertAll();
RunLoop(options, sim);
}
private static void RunLoop(BenchOptions options, Scenario2D sim)
{
Console.WriteLine($"took {sim.InsertMilliseconds}ms to insert {sim.AgentCount} entities");
Console.WriteLine($"{sim.Hshg.GridCount} grids\n");
Console.WriteLine("--- | average | sd | +- 0.1ms | +- 0.4ms | +- 0.7ms | +- 1.0ms | col stats |");
bool interrupted = false;
Console.CancelKeyPress += (_, e) =>
{
interrupted = true;
e.Cancel = true;
};
var stats = new LatencyStats(options.Samples);
int windows = 0;
while (!interrupted)
{
stats.Add(sim.Tick());
if (!stats.Full)
{
continue;
}
stats.PrintAndReset(sim.MaybeCollisions, sim.Collisions);
sim.ResetCollisionStats();
windows++;
if (options.Windows > 0 && windows >= options.Windows)
{
break;
}
}
}
}
internal static class ConsoleBench3D
{
public static void Run(BenchOptions options)
{
int agents = options.Lite ? 100_000 : 300_000;
var sim = new Scenario3D(agents, 128, 16, 7, options.Seed ?? Environment.TickCount);
sim.InsertAll();
RunLoop(options, sim);
}
private static void RunLoop(BenchOptions options, Scenario3D sim)
{
Console.WriteLine($"took {sim.InsertMilliseconds}ms to insert {sim.AgentCount} entities");
Console.WriteLine($"{sim.Hshg.GridCount} grids\n");
Console.WriteLine("--- | average | sd | +- 0.1ms | +- 0.4ms | +- 0.7ms | +- 1.0ms | col stats |");
bool interrupted = false;
Console.CancelKeyPress += (_, e) =>
{
interrupted = true;
e.Cancel = true;
};
var stats = new LatencyStats(options.Samples);
int windows = 0;
while (!interrupted)
{
stats.Add(sim.Tick());
if (!stats.Full)
{
continue;
}
stats.PrintAndReset(sim.MaybeCollisions, sim.Collisions);
sim.ResetCollisionStats();
windows++;
if (options.Windows > 0 && windows >= options.Windows)
{
break;
}
}
}
}
internal sealed class Scenario2D
{
private const int QueryWidth = 1920;
private const int QueryHeight = 1080;
private readonly float[] _initData;
private Ball2D[] _balls;
private Ball2D[] _ballsScratch;
private int _queriedLen;
private readonly int _queriesPerTick;
private readonly int _arenaWidth;
private readonly int _arenaHeight;
private readonly float _agentRadius;
private ulong _maybeCollisions;
private ulong _collisions;
public Scenario2D(int agentCount, int cellsSide, int cellSize, float agentRadius, int seed)
{
AgentCount = agentCount;
_agentRadius = agentRadius;
_arenaWidth = checked(cellsSide * cellSize);
_arenaHeight = checked(cellsSide * cellSize);
_queriesPerTick = 255 >> 2;
_initData = GC.AllocateUninitializedArray<float>(agentCount * 3);
_balls = GC.AllocateUninitializedArray<Ball2D>(agentCount);
_ballsScratch = GC.AllocateUninitializedArray<Ball2D>(agentCount);
Hshg = new Hshg2D(cellsSide, cellSize);
Hshg.EnsureCapacity(agentCount + 1);
Initialize(seed);
}
public int AgentCount { get; }
public Hshg2D Hshg { get; }
public long InsertMilliseconds { get; private set; }
public ulong MaybeCollisions => _maybeCollisions;
public ulong Collisions => _collisions;
public void InsertAll()
{
long start = Stopwatch.GetTimestamp();
for (int i = 0; i < AgentCount; i++)
{
Hshg.Insert(_initData[i * 3], _initData[i * 3 + 2], _initData[i * 3 + 1], i);
}
InsertMilliseconds = (long)Stopwatch.GetElapsedTime(start).TotalMilliseconds;
}
public TickTimings Tick()
{
long optStart = Stopwatch.GetTimestamp();
Hshg.OptimizeAndRemapReferences(_balls, _ballsScratch);
(_balls, _ballsScratch) = (_ballsScratch, _balls);
long colStart = Stopwatch.GetTimestamp();
var collider = new Collider2D(_balls);
Hshg.Collide(ref collider);
_maybeCollisions += collider.MaybeCollisions;
_collisions += collider.Collisions;
long qryStart = Stopwatch.GetTimestamp();
QueryBatch();
long updStart = Stopwatch.GetTimestamp();
var updater = new Updater2D(this);
Hshg.UpdateDenseAndMove(ref updater);
long end = Stopwatch.GetTimestamp();
return new TickTimings(
ElapsedMs(optStart, colStart),
ElapsedMs(colStart, qryStart),
ElapsedMs(qryStart, updStart),
ElapsedMs(updStart, end));
}
public void ResetCollisionStats()
{
_maybeCollisions = 0;
_collisions = 0;
}
private void Initialize(int seed)
{
var random = new Random(seed);
for (int i = 0; i < AgentCount; i++)
{
_initData[i * 3] = random.NextSingle() * _arenaWidth;
_initData[i * 3 + 1] = _agentRadius;
_initData[i * 3 + 2] = random.NextSingle() * _arenaHeight;
_balls[i].Vx = random.NextSingle() - 0.5f;
_balls[i].Vy = random.NextSingle() - 0.5f;
}
}
private void QueryBatch()
{
var query = new Query2D(this);
int ptr = _queriedLen * 3;
for (int i = 0; i < _queriesPerTick; i++)
{
Hshg.Query(
_initData[ptr] - QueryWidth * 0.5f,
_initData[ptr + 2] - QueryHeight * 0.5f,
_initData[ptr] + QueryWidth * 0.5f,
_initData[ptr + 2] + QueryHeight * 0.5f,
ref query);
ptr++;
if (ptr + 2 >= _initData.Length)
{
ptr = 0;
}
}
}
private static double ElapsedMs(long start, long end) => (end - start) * 1000.0 / Stopwatch.Frequency;
private struct Updater2D(Scenario2D scenario) : IHshgDenseUpdater2D
{
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Update(ref HshgEntity2D entity)
{
ref Ball2D ball = ref scenario._balls[entity.Reference];
entity.X += ball.Vx;
if (entity.X < entity.Radius)
{
ball.Vx *= 0.9f;
ball.Vx += 1.0f;
}
else if (entity.X + entity.Radius >= scenario._arenaWidth)
{
ball.Vx *= 0.9f;
ball.Vx -= 1.0f;
}
entity.Y += ball.Vy;
if (entity.Y < entity.Radius)
{
ball.Vy *= 0.9f;
ball.Vy += 1.0f;
}
else if (entity.Y + entity.Radius >= scenario._arenaHeight)
{
ball.Vy *= 0.9f;
ball.Vy -= 1.0f;
}
}
}
private struct Collider2D(Ball2D[] balls) : IHshgCollider2D
{
public ulong MaybeCollisions;
public ulong Collisions;
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Collide(Hshg2D hshg, in HshgEntity2D a, in HshgEntity2D b)
{
_ = hshg;
MaybeCollisions++;
float dx = a.X - b.X;
float dy = a.Y - b.Y;
float dr = a.Radius + b.Radius;
float dist = dx * dx + dy * dy;
if (dist <= dr * dr)
{
Collisions++;
float mag = 1.0f / float.Sqrt(dist);
dx *= mag;
dy *= mag;
ref Ball2D ballsBase = ref MemoryMarshal.GetArrayDataReference(balls);
ref Ball2D ballA = ref Unsafe.Add(ref ballsBase, a.Reference);
ref Ball2D ballB = ref Unsafe.Add(ref ballsBase, b.Reference);
ballA.Vx *= 0.75f;
ballA.Vx += dx;
ballA.Vy *= 0.75f;
ballA.Vy += dy;
ballB.Vx *= 0.75f;
ballB.Vx -= dx;
ballB.Vy *= 0.75f;
ballB.Vy -= dy;
}
}
}
private struct Query2D(Scenario2D scenario) : IHshgQuery2D
{
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Query(Hshg2D hshg, in HshgEntity2D entity)
{
_ = hshg;
_ = entity.Reference;
scenario._queriedLen++;
scenario._queriedLen = (scenario._queriedLen + 1) % scenario.AgentCount;
}
}
}
internal sealed class Scenario3D
{
private const int QueryWidth = 1920;
private const int QueryHeight = 1080;
private readonly float[] _initData;
private Ball3D[] _balls;
private Ball3D[] _ballsScratch;
private int _queriedLen;
private readonly int _queriesPerTick;
private readonly int _arenaWidth;
private readonly int _arenaHeight;
private readonly int _arenaDepth;
private readonly float _agentRadius;
private ulong _maybeCollisions;
private ulong _collisions;
public Scenario3D(int agentCount, int cellsSide, int cellSize, float agentRadius, int seed)
{
AgentCount = agentCount;
_agentRadius = agentRadius;
_arenaWidth = checked(cellsSide * cellSize);
_arenaHeight = checked(cellsSide * cellSize);
_arenaDepth = checked(cellsSide * cellSize);
_queriesPerTick = 255 >> 4;
_initData = GC.AllocateUninitializedArray<float>(agentCount * 4);
_balls = GC.AllocateUninitializedArray<Ball3D>(agentCount);
_ballsScratch = GC.AllocateUninitializedArray<Ball3D>(agentCount);
Hshg = new Hshg3D(cellsSide, cellSize);
Hshg.EnsureCapacity(agentCount + 1);
Initialize(seed);
}
public int AgentCount { get; }
public Hshg3D Hshg { get; }
public long InsertMilliseconds { get; private set; }
public ulong MaybeCollisions => _maybeCollisions;
public ulong Collisions => _collisions;
public void InsertAll()
{
long start = Stopwatch.GetTimestamp();
for (int i = 0; i < AgentCount; i++)
{
Hshg.Insert(_initData[i * 4], _initData[i * 4 + 2], _initData[i * 4 + 3], _initData[i * 4 + 1], i);
}
InsertMilliseconds = (long)Stopwatch.GetElapsedTime(start).TotalMilliseconds;
}
public TickTimings Tick()
{
long optStart = Stopwatch.GetTimestamp();
Hshg.OptimizeAndRemapReferences(_balls, _ballsScratch);
(_balls, _ballsScratch) = (_ballsScratch, _balls);
long colStart = Stopwatch.GetTimestamp();
var collider = new Collider3D(_balls);
Hshg.Collide(ref collider);
_maybeCollisions += collider.MaybeCollisions;
_collisions += collider.Collisions;
long qryStart = Stopwatch.GetTimestamp();
QueryBatch();
long updStart = Stopwatch.GetTimestamp();
var updater = new Updater3D(this);
Hshg.UpdateDenseAndMove(ref updater);
long end = Stopwatch.GetTimestamp();
return new TickTimings(
ElapsedMs(optStart, colStart),
ElapsedMs(colStart, qryStart),
ElapsedMs(qryStart, updStart),
ElapsedMs(updStart, end));
}
public void ResetCollisionStats()
{
_maybeCollisions = 0;
_collisions = 0;
}
private void Initialize(int seed)
{
var random = new Random(seed);
for (int i = 0; i < AgentCount; i++)
{
_initData[i * 4] = random.NextSingle() * _arenaWidth;
_initData[i * 4 + 1] = _agentRadius;
_initData[i * 4 + 2] = random.NextSingle() * _arenaHeight;
_initData[i * 4 + 3] = random.NextSingle() * _arenaDepth;
_balls[i].Vx = random.NextSingle() - 0.5f;
_balls[i].Vy = random.NextSingle() - 0.5f;
_balls[i].Vz = random.NextSingle() - 0.5f;
}
}
private void QueryBatch()
{
var query = new Query3D(this);
int ptr = _queriedLen * 4;
for (int i = 0; i < _queriesPerTick; i++)
{
Hshg.Query(
_initData[ptr] - QueryWidth * 0.5f,
_initData[ptr + 2] - QueryHeight * 0.5f,
_initData[ptr + 3] - QueryHeight * 0.5f,
_initData[ptr] + QueryWidth * 0.5f,
_initData[ptr + 2] + QueryHeight * 0.5f,
_initData[ptr + 3] + QueryHeight * 0.5f,
ref query);
ptr++;
if (ptr + 3 >= _initData.Length)
{
ptr = 0;
}
}
}
private static double ElapsedMs(long start, long end) => (end - start) * 1000.0 / Stopwatch.Frequency;
private struct Updater3D(Scenario3D scenario) : IHshgDenseUpdater3D
{
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Update(ref HshgEntity3D entity)
{
ref Ball3D ball = ref scenario._balls[entity.Reference];
entity.X += ball.Vx;
if (entity.X < entity.Radius)
{
ball.Vx *= 0.9f;
ball.Vx += 1.0f;
}
else if (entity.X + entity.Radius >= scenario._arenaWidth)
{
ball.Vx *= 0.9f;
ball.Vx -= 1.0f;
}
entity.Y += ball.Vy;
if (entity.Y < entity.Radius)
{
ball.Vy *= 0.9f;
ball.Vy += 1.0f;
}
else if (entity.Y + entity.Radius >= scenario._arenaHeight)
{
ball.Vy *= 0.9f;
ball.Vy -= 1.0f;
}
entity.Z += ball.Vz;
if (entity.Z < entity.Radius)
{
ball.Vz *= 0.9f;
ball.Vz += 1.0f;
}
else if (entity.Z + entity.Radius >= scenario._arenaDepth)
{
ball.Vz *= 0.9f;
ball.Vz -= 1.0f;
}
}
}
private struct Collider3D(Ball3D[] balls) : IHshgCollider3D
{
public ulong MaybeCollisions;
public ulong Collisions;
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Collide(Hshg3D hshg, in HshgEntity3D a, in HshgEntity3D b)
{
_ = hshg;
MaybeCollisions++;
float dx = a.X - b.X;
float dy = a.Y - b.Y;
float dz = a.Z - b.Z;
float dr = a.Radius + b.Radius;
float dist = dx * dx + dy * dy + dz * dz;
if (dist <= dr * dr)
{
Collisions++;
float mag = 1.0f / float.Sqrt(dist);
dx *= mag;
dy *= mag;
dz *= mag;
ref Ball3D ballsBase = ref MemoryMarshal.GetArrayDataReference(balls);
ref Ball3D ballA = ref Unsafe.Add(ref ballsBase, a.Reference);
ref Ball3D ballB = ref Unsafe.Add(ref ballsBase, b.Reference);
ballA.Vx *= 0.75f;
ballA.Vx += dx;
ballA.Vy *= 0.75f;
ballA.Vy += dy;
ballA.Vz *= 0.75f;
ballA.Vz += dz;
ballB.Vx *= 0.75f;
ballB.Vx -= dx;
ballB.Vy *= 0.75f;
ballB.Vy -= dy;
ballB.Vz *= 0.75f;
ballB.Vz -= dz;
}
}
}
private struct Query3D(Scenario3D scenario) : IHshgQuery3D
{
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Query(Hshg3D hshg, in HshgEntity3D entity)
{
_ = hshg;
_ = entity.Reference;
scenario._queriedLen++;
scenario._queriedLen = (scenario._queriedLen + 1) % scenario.AgentCount;
}
}
}
internal readonly record struct TickTimings(double OptimizeMs, double CollideMs, double QueryMs, double UpdateMs);
internal sealed class LatencyStats
{
private readonly TickTimings[] _samples;
private int _index;
public LatencyStats(int sampleCount)
{
_samples = new TickTimings[sampleCount];
}
public bool Full => _index == _samples.Length;
public void Add(TickTimings timings)
{
_samples[_index++] = timings;
}
public void PrintAndReset(ulong maybeCollisions, ulong collisions)
{
double optAvg = Average(static t => t.OptimizeMs);
double colAvg = Average(static t => t.CollideMs);
double qryAvg = Average(static t => t.QueryMs);
double updAvg = Average(static t => t.UpdateMs);
double colSd = StandardDeviation(static t => t.CollideMs, colAvg);
double updSd = StandardDeviation(static t => t.UpdateMs, updAvg);
Console.WriteLine("------------------------------------------------------------------------------");
Console.WriteLine($"opt | {optAvg,5:0.00}ms | ---- | -------- | -------- | -------- | -------- | attempted |");
Console.WriteLine($"col | {colAvg,5:0.00}ms | {colSd,4:0.00} | {Within(static t => t.CollideMs, colAvg, 0.1),5:0.0}% | {Within(static t => t.CollideMs, colAvg, 0.4),5:0.0}% | {Within(static t => t.CollideMs, colAvg, 0.7),5:0.0}% | {Within(static t => t.CollideMs, colAvg, 1.0),5:0.0}% | {maybeCollisions,9} |");
Console.WriteLine($"qry | {qryAvg,5:0.00}ms | ---- | -------- | -------- | -------- | -------- | |");
Console.WriteLine($"upd | {updAvg,5:0.00}ms | {updSd,4:0.00} | {Within(static t => t.UpdateMs, updAvg, 0.1),5:0.0}% | {Within(static t => t.UpdateMs, updAvg, 0.4),5:0.0}% | {Within(static t => t.UpdateMs, updAvg, 0.7),5:0.0}% | {Within(static t => t.UpdateMs, updAvg, 1.0),5:0.0}% | succeeded |");
Console.WriteLine($"all | {optAvg + colAvg + qryAvg + updAvg,5:0.00}ms | ---- | -------- | -------- | -------- | -------- | {collisions,9} |");
_index = 0;
}
private double Average(Func<TickTimings, double> selector)
{
double sum = 0;
for (int i = 0; i < _samples.Length; i++)
{
sum += selector(_samples[i]);
}
return sum / _samples.Length;
}
private double StandardDeviation(Func<TickTimings, double> selector, double average)
{
double sum = 0;
for (int i = 0; i < _samples.Length; i++)
{
double diff = selector(_samples[i]) - average;
sum += diff * diff;
}
return Math.Sqrt(sum / _samples.Length);
}
private double Within(Func<TickTimings, double> selector, double average, double window)
{
int count = 0;
for (int i = 0; i < _samples.Length; i++)
{
double value = selector(_samples[i]);
if (value >= average - window && value <= average + window)
{
count++;
}
}
return count * 100.0 / _samples.Length;
}
}
[StructLayout(LayoutKind.Sequential)]
internal struct HshgEntity2D
{
public int Cell;
public byte Grid;
public int Next;
public int Prev;
public int Reference;
public float X;
public float Y;
public float Radius;
}
[StructLayout(LayoutKind.Sequential)]
internal struct HshgEntity3D
{
public int Cell;
public byte Grid;
public int Next;
public int Prev;
public int Reference;
public float X;
public float Y;
public float Z;
public float Radius;
}
internal interface IHshgDenseUpdater2D
{
void Update(ref HshgEntity2D entity);
}
internal interface IHshgCollider2D
{
void Collide(Hshg2D hshg, in HshgEntity2D a, in HshgEntity2D b);
}
internal interface IHshgQuery2D
{
void Query(Hshg2D hshg, in HshgEntity2D entity);
}
internal interface IHshgDenseUpdater3D
{
void Update(ref HshgEntity3D entity);
}
internal interface IHshgCollider3D
{
void Collide(Hshg3D hshg, in HshgEntity3D a, in HshgEntity3D b);
}
internal interface IHshgQuery3D
{
void Query(Hshg3D hshg, in HshgEntity3D entity);
}
internal sealed class Hshg2D
{
private const int InvalidCell = int.MaxValue;
private HshgEntity2D[] _entities;
private HshgEntity2D[]? _optimizeBuffer;
private readonly int[] _cellHeads;
private readonly Grid[] _grids;
private readonly int _cellLog;
private readonly int _cellSize;
private readonly int _gridSize;
private readonly float _inverseGridSize;
private int _entitiesUsed;
private uint _oldCache;
private uint _newCache;
public Hshg2D(int side, int cellSize)
{
if (!IsPowerOfTwo(side) || !IsPowerOfTwo(cellSize))
{
throw new ArgumentException("side and cellSize must be positive powers of two.");
}
int cellsLen = MaxCells(side);
int gridsLen = MaxGrids(side);
_entities = new HshgEntity2D[1];
_cellHeads = new int[cellsLen];
_grids = GC.AllocateUninitializedArray<Grid>(gridsLen);
_cellLog = 31 - BitOperations.TrailingZeroCount((uint)cellSize);
_cellSize = cellSize;
_gridSize = checked(side * cellSize);
_inverseGridSize = 1.0f / _gridSize;
_entitiesUsed = 1;
int cellOffset = 0;
int gridSide = side;
int gridCellSize = cellSize;
for (int i = 0; i < gridsLen; i++)
{
_grids[i] = new Grid
{
CellOffset = cellOffset,
CellsSide = gridSide,
CellsMask = gridSide - 1,
Cells2DLog = BitOperations.TrailingZeroCount((uint)gridSide),
InverseCellSize = 1.0f / gridCellSize,
};
cellOffset += checked(gridSide * gridSide);
gridSide >>= 1;
gridCellSize <<= 1;
}
}
public int GridCount => _grids.Length;
public void EnsureCapacity(int capacityIncludingSentinel)
{
if (capacityIncludingSentinel <= _entities.Length)
{
return;
}
HshgEntity2D[] entities = GC.AllocateUninitializedArray<HshgEntity2D>(capacityIncludingSentinel);
Array.Copy(_entities, entities, _entitiesUsed);
_entities = entities;
if (_optimizeBuffer is not null && _optimizeBuffer.Length < capacityIncludingSentinel)
{
_optimizeBuffer = null;
}
}
public void Insert(float x, float y, float radius, int reference)
{
int index = GetEntityIndex();
ref HshgEntity2D entity = ref _entities[index];
entity.Grid = GetGrid(radius);
entity.Reference = reference;
entity.X = x;
entity.Y = y;
entity.Radius = radius;
Reinsert(index);
}
public void UpdateDenseAndMove<TUpdater>(ref TUpdater updater)
where TUpdater : struct, IHshgDenseUpdater2D
{
for (int entityIndex = 1; entityIndex < _entitiesUsed; entityIndex++)
{
ref HshgEntity2D entity = ref _entities[entityIndex];
if (entity.Cell == InvalidCell)
{
continue;
}
updater.Update(ref entity);
ref Grid grid = ref _grids[entity.Grid];
int newCell = GetCell(in grid, entity.X, entity.Y);
if (entity.Cell != newCell)
{
RemoveLight(entityIndex);
Reinsert(entityIndex);
}
}
}
public void Collide<TCollider>(ref TCollider collider)
where TCollider : struct, IHshgCollider2D
{
UpdateCache();
ref HshgEntity2D entities = ref MemoryMarshal.GetArrayDataReference(_entities);
ref int cellHeads = ref MemoryMarshal.GetArrayDataReference(_cellHeads);
ref Grid grids = ref MemoryMarshal.GetArrayDataReference(_grids);
int entitiesUsed = _entitiesUsed;
for (int entityIndex = 1; entityIndex < entitiesUsed; entityIndex++)
{
ref HshgEntity2D entity = ref Unsafe.Add(ref entities, entityIndex);
if (entity.Cell == InvalidCell)
{
continue;
}
int gridIndex = entity.Grid;
ref Grid grid = ref Unsafe.Add(ref grids, gridIndex);
int cellX = entity.Cell & grid.CellsMask;
int cellY = entity.Cell >> grid.Cells2DLog;
int head = entity.Next;
while (head != 0)
{
ref HshgEntity2D other = ref Unsafe.Add(ref entities, head);
collider.Collide(this, in entity, in other);
head = other.Next;
}
if (cellX != grid.CellsMask)
{
head = Unsafe.Add(ref cellHeads, grid.CellOffset + entity.Cell + 1);
while (head != 0)
{
ref HshgEntity2D other = ref Unsafe.Add(ref entities, head);
collider.Collide(this, in entity, in other);
head = other.Next;
}
}
if (cellY != grid.CellsMask)
{
int cell = grid.CellOffset + entity.Cell + grid.CellsSide;
if (cellX != 0)
{
head = Unsafe.Add(ref cellHeads, cell - 1);
while (head != 0)
{
ref HshgEntity2D other = ref Unsafe.Add(ref entities, head);
collider.Collide(this, in entity, in other);
head = other.Next;
}
}
head = Unsafe.Add(ref cellHeads, cell);
while (head != 0)
{
ref HshgEntity2D other = ref Unsafe.Add(ref entities, head);
collider.Collide(this, in entity, in other);
head = other.Next;
}
if (cellX != grid.CellsMask)
{
head = Unsafe.Add(ref cellHeads, cell + 1);
while (head != 0)
{
ref HshgEntity2D other = ref Unsafe.Add(ref entities, head);
collider.Collide(this, in entity, in other);
head = other.Next;
}
}
}
while (grid.Shift != 0)
{
int shift = grid.Shift;
cellX >>= shift;
cellY >>= shift;
gridIndex += shift;
grid = ref Unsafe.Add(ref grids, gridIndex);
int minCellX = cellX != 0 ? cellX - 1 : 0;
int minCellY = cellY != 0 ? cellY - 1 : 0;
int maxCellX = cellX != grid.CellsMask ? cellX + 1 : cellX;
int maxCellY = cellY != grid.CellsMask ? cellY + 1 : cellY;
for (int y = minCellY; y <= maxCellY; y++)
{
for (int x = minCellX; x <= maxCellX; x++)
{
head = Unsafe.Add(ref cellHeads, grid.CellOffset + (x | (y << grid.Cells2DLog)));
while (head != 0)
{
ref HshgEntity2D other = ref Unsafe.Add(ref entities, head);
collider.Collide(this, in entity, in other);
head = other.Next;
}
}
}
}
}
}
public void Query<TQuery>(float minX, float minY, float maxX, float maxY, ref TQuery query)
where TQuery : struct, IHshgQuery2D
{
UpdateCache();
(int xStart, int xEnd) = MapPosition(minX, maxX);
(int yStart, int yEnd) = MapPosition(minY, maxY);
int gridIndex = 0;
int shift = 0;
while (gridIndex < _grids.Length && _grids[gridIndex].EntitiesLen == 0)
{
gridIndex++;
shift++;
}
if (gridIndex == _grids.Length)
{
return;
}
xStart >>= shift;
yStart >>= shift;
xEnd >>= shift;
yEnd >>= shift;
while (true)
{
ref Grid grid = ref _grids[gridIndex];
int sx = xStart != 0 ? xStart - 1 : 0;
int sy = yStart != 0 ? yStart - 1 : 0;
int ex = xEnd != grid.CellsMask ? xEnd + 1 : xEnd;
int ey = yEnd != grid.CellsMask ? yEnd + 1 : yEnd;
for (int y = sy; y <= ey; y++)
{
for (int x = sx; x <= ex; x++)
{
for (int j = _cellHeads[grid.CellOffset + (x | (y << grid.Cells2DLog))]; j != 0;)
{
ref HshgEntity2D entity = ref _entities[j];
if (entity.X + entity.Radius >= minX &&
entity.X - entity.Radius <= maxX &&
entity.Y + entity.Radius >= minY &&
entity.Y - entity.Radius <= maxY)
{
query.Query(this, in entity);
}
j = entity.Next;
}
}
}
if (grid.Shift == 0)
{
break;
}
shift = grid.Shift;
xStart >>= shift;
yStart >>= shift;
xEnd >>= shift;
yEnd >>= shift;
gridIndex += shift;
}
}
public void OptimizeAndRemapReferences<T>(T[] sourceReferences, T[] destinationReferences)
{
HshgEntity2D[] entities = _optimizeBuffer is { } buffer && buffer.Length >= _entities.Length
? buffer
: GC.AllocateUninitializedArray<HshgEntity2D>(_entities.Length);
HshgEntity2D[] oldEntities = _entities;
ref int cellHeads = ref MemoryMarshal.GetArrayDataReference(_cellHeads);
ref HshgEntity2D source = ref MemoryMarshal.GetArrayDataReference(oldEntities);
ref HshgEntity2D destination = ref MemoryMarshal.GetArrayDataReference(entities);
ref T destinationReference = ref MemoryMarshal.GetArrayDataReference(destinationReferences);
int index = 1;
int referenceIndex = 0;
for (int cell = 0; cell < _cellHeads.Length; cell++)
{
ref int cellHead = ref Unsafe.Add(ref cellHeads, cell);
int entityIndex = cellHead;
if (entityIndex == 0)
{
continue;
}
cellHead = index;
while (true)
{
ref HshgEntity2D entity = ref Unsafe.Add(ref destination, index);
entity = Unsafe.Add(ref source, entityIndex);
Unsafe.Add(ref destinationReference, referenceIndex) = sourceReferences[entity.Reference];
entity.Reference = referenceIndex++;
if (entity.Prev != 0)
{
entity.Prev = index - 1;
}
index++;
if (entity.Next != 0)
{
entityIndex = entity.Next;
entity.Next = index;
}
else
{
break;
}
}
}
_optimizeBuffer = oldEntities;
_entities = entities;
_entitiesUsed = index;
}
private int GetEntityIndex()
{
if (_entitiesUsed == _entities.Length)
{
EnsureCapacity(_entities.Length << 1);
}
return _entitiesUsed++;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void Reinsert(int index)
{
ref HshgEntity2D entity = ref _entities[index];
ref Grid grid = ref _grids[entity.Grid];
entity.Cell = GetCell(in grid, entity.X, entity.Y);
int cellIndex = grid.CellOffset + entity.Cell;
entity.Next = _cellHeads[cellIndex];
if (entity.Next != 0)
{
_entities[entity.Next].Prev = index;
}
entity.Prev = 0;
_cellHeads[cellIndex] = index;
if (grid.EntitiesLen == 0)
{
_newCache |= 1u << entity.Grid;
}
grid.EntitiesLen++;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void RemoveLight(int index)
{
ref HshgEntity2D entity = ref _entities[index];
ref Grid grid = ref _grids[entity.Grid];
if (entity.Prev == 0)
{
_cellHeads[grid.CellOffset + entity.Cell] = entity.Next;
}
else
{
_entities[entity.Prev].Next = entity.Next;
}
_entities[entity.Next].Prev = entity.Prev;
grid.EntitiesLen--;
if (grid.EntitiesLen == 0)
{
_newCache ^= 1u << entity.Grid;
}
}
private void UpdateCache()
{
if (_oldCache == _newCache)
{
return;
}
_oldCache = _newCache;
for (int i = 0; i < _grids.Length; i++)
{
_grids[i].Shift = 0;
}
int oldGrid = 0;
while (oldGrid < _grids.Length && _grids[oldGrid].EntitiesLen == 0)
{
oldGrid++;
}
if (oldGrid == _grids.Length)
{
return;
}
byte shift = 1;
for (int newGrid = oldGrid + 1; newGrid < _grids.Length; newGrid++)
{
if (_grids[newGrid].EntitiesLen == 0)
{
shift++;
continue;
}
_grids[oldGrid].Shift = shift;
oldGrid = newGrid;
shift = 1;
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private byte GetGrid(float radius)
{
uint rounded = (uint)(radius + radius);
if (rounded < (uint)_cellSize)
{
return 0;
}
int grid = _cellLog - BitOperations.LeadingZeroCount(rounded) + 1;
return (byte)Math.Min(grid, _grids.Length - 1);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private static int GetCell1D(in Grid grid, float x)
{
int cell = (int)(MathF.Abs(x) * grid.InverseCellSize);
return (cell & grid.CellsSide) != 0
? grid.CellsMask - (cell & grid.CellsMask)
: cell & grid.CellsMask;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private static int GetCell(in Grid grid, float x, float y) =>
GetCell1D(in grid, x) | (GetCell1D(in grid, y) << grid.Cells2DLog);
private (int Start, int End) MapPosition(float x1Raw, float x2Raw)
{
float x1;
float x2;
if (x1Raw < 0)
{
float shift = ((((int)(-x1Raw * _inverseGridSize)) << 1) + 2) * _gridSize;
x1 = x1Raw + shift;
x2 = x2Raw + shift;
}
else
{
x1 = x1Raw;
x2 = x2Raw;
}
ref Grid grid = ref _grids[0];
int folds = (int)((x2 - ((int)(x1 * _inverseGridSize)) * _gridSize) * _inverseGridSize);
switch (folds)
{
case 0:
{
int cell = GetCell1D(in grid, x1);
int end = GetCell1D(in grid, x2);
return (Math.Min(cell, end), Math.Max(cell, end));
}
case 1:
{
int cell = (int)(MathF.Abs(x1) * grid.InverseCellSize);
int end = GetCell1D(in grid, x2);
return (cell & grid.CellsSide) != 0
? (0, Math.Max(grid.CellsMask - (cell & grid.CellsMask), end))
: (Math.Min(cell & grid.CellsMask, end), grid.CellsMask);
}
default:
return (0, grid.CellsMask);
}
}
private static bool IsPowerOfTwo(int value) => value > 0 && (value & (value - 1)) == 0;
private static int MaxGrids(int side)
{
int grids = 0;
do
{
grids++;
side >>= 1;
}
while (side >= 2);
return grids;
}
private static int MaxCells(int side)
{
long cells = 0;
do
{
cells += (long)side * side;
side >>= 1;
}
while (side >= 2);
return (int)cells;
}
private struct Grid
{
public int CellOffset;
public int CellsSide;
public int CellsMask;
public int Cells2DLog;
public byte Shift;
public float InverseCellSize;
public int EntitiesLen;
}
}
internal sealed class Hshg3D
{
private const int InvalidCell = int.MaxValue;
private HshgEntity3D[] _entities;
private HshgEntity3D[]? _optimizeBuffer;
private readonly int[] _cellHeads;
private readonly Grid[] _grids;
private readonly int _cellLog;
private readonly int _cellSize;
private readonly int _gridSize;
private readonly float _inverseGridSize;
private int _entitiesUsed;
private uint _oldCache;
private uint _newCache;
public Hshg3D(int side, int cellSize)
{
if (!IsPowerOfTwo(side) || !IsPowerOfTwo(cellSize))
{
throw new ArgumentException("side and cellSize must be positive powers of two.");
}
int cellsLen = MaxCells(side);
int gridsLen = MaxGrids(side);
_entities = new HshgEntity3D[1];
_cellHeads = new int[cellsLen];
_grids = GC.AllocateUninitializedArray<Grid>(gridsLen);
_cellLog = 31 - BitOperations.TrailingZeroCount((uint)cellSize);
_cellSize = cellSize;
_gridSize = checked(side * cellSize);
_inverseGridSize = 1.0f / _gridSize;
_entitiesUsed = 1;
int cellOffset = 0;
int gridSide = side;
int gridCellSize = cellSize;
for (int i = 0; i < gridsLen; i++)
{
int cellsSq = checked(gridSide * gridSide);
_grids[i] = new Grid
{
CellOffset = cellOffset,
CellsSide = gridSide,
CellsSq = cellsSq,
CellsMask = gridSide - 1,
Cells2DLog = BitOperations.TrailingZeroCount((uint)gridSide),
Cells3DLog = BitOperations.TrailingZeroCount((uint)gridSide) << 1,
InverseCellSize = 1.0f / gridCellSize,
};
cellOffset += checked(cellsSq * gridSide);
gridSide >>= 1;
gridCellSize <<= 1;
}
}
public int GridCount => _grids.Length;
public void EnsureCapacity(int capacityIncludingSentinel)
{
if (capacityIncludingSentinel <= _entities.Length)
{
return;
}
HshgEntity3D[] entities = GC.AllocateUninitializedArray<HshgEntity3D>(capacityIncludingSentinel);
Array.Copy(_entities, entities, _entitiesUsed);
_entities = entities;
if (_optimizeBuffer is not null && _optimizeBuffer.Length < capacityIncludingSentinel)
{
_optimizeBuffer = null;
}
}
public void Insert(float x, float y, float z, float radius, int reference)
{
int index = GetEntityIndex();
ref HshgEntity3D entity = ref _entities[index];
entity.Grid = GetGrid(radius);
entity.Reference = reference;
entity.X = x;
entity.Y = y;
entity.Z = z;
entity.Radius = radius;
Reinsert(index);
}
public void UpdateDenseAndMove<TUpdater>(ref TUpdater updater)
where TUpdater : struct, IHshgDenseUpdater3D
{
for (int entityIndex = 1; entityIndex < _entitiesUsed; entityIndex++)
{
ref HshgEntity3D entity = ref _entities[entityIndex];
if (entity.Cell == InvalidCell)
{
continue;
}
updater.Update(ref entity);
ref Grid grid = ref _grids[entity.Grid];
int newCell = GetCell(in grid, entity.X, entity.Y, entity.Z);
if (entity.Cell != newCell)
{
RemoveLight(entityIndex);
Reinsert(entityIndex);
}
}
}
public void Collide<TCollider>(ref TCollider collider)
where TCollider : struct, IHshgCollider3D
{
UpdateCache();
for (int entityIndex = 1; entityIndex < _entitiesUsed; entityIndex++)
{
ref HshgEntity3D entity = ref _entities[entityIndex];
if (entity.Cell == InvalidCell)
{
continue;
}
int gridIndex = entity.Grid;
ref Grid grid = ref _grids[gridIndex];
int cellX = entity.Cell & grid.CellsMask;
int cellY = (entity.Cell >> grid.Cells2DLog) & grid.CellsMask;
int cellZ = entity.Cell >> grid.Cells3DLog;
if (cellZ != 0)
{
if (cellY != 0)
{
int cell = grid.CellOffset + entity.Cell - grid.CellsSq - grid.CellsSide;
if (cellX != 0)
{
CollideChain(ref collider, in entity, _cellHeads[cell - 1]);
}
CollideChain(ref collider, in entity, _cellHeads[cell]);
if (cellX != grid.CellsMask)
{
CollideChain(ref collider, in entity, _cellHeads[cell + 1]);
}
}
{
int cell = grid.CellOffset + entity.Cell - grid.CellsSq;
if (cellX != 0)
{
CollideChain(ref collider, in entity, _cellHeads[cell - 1]);
}
CollideChain(ref collider, in entity, _cellHeads[cell]);
if (cellX != grid.CellsMask)
{
CollideChain(ref collider, in entity, _cellHeads[cell + 1]);
}
}
if (cellY != grid.CellsMask)
{
int cell = grid.CellOffset + entity.Cell - grid.CellsSq + grid.CellsSide;
if (cellX != 0)
{
CollideChain(ref collider, in entity, _cellHeads[cell - 1]);
}
CollideChain(ref collider, in entity, _cellHeads[cell]);
if (cellX != grid.CellsMask)
{
CollideChain(ref collider, in entity, _cellHeads[cell + 1]);
}
}
}
CollideChain(ref collider, in entity, entity.Next);
if (cellX != grid.CellsMask)
{
CollideChain(ref collider, in entity, _cellHeads[grid.CellOffset + entity.Cell + 1]);
}
if (cellY != grid.CellsMask)
{
int cell = grid.CellOffset + entity.Cell + grid.CellsSide;
if (cellX != 0)
{
CollideChain(ref collider, in entity, _cellHeads[cell - 1]);
}
CollideChain(ref collider, in entity, _cellHeads[cell]);
if (cellX != grid.CellsMask)
{
CollideChain(ref collider, in entity, _cellHeads[cell + 1]);
}
}
while (grid.Shift != 0)
{
int shift = grid.Shift;
cellX >>= shift;
cellY >>= shift;
cellZ >>= shift;
gridIndex += shift;
grid = ref _grids[gridIndex];
int minCellX = cellX != 0 ? cellX - 1 : 0;
int minCellY = cellY != 0 ? cellY - 1 : 0;
int minCellZ = cellZ != 0 ? cellZ - 1 : 0;
int maxCellX = cellX != grid.CellsMask ? cellX + 1 : cellX;
int maxCellY = cellY != grid.CellsMask ? cellY + 1 : cellY;
int maxCellZ = cellZ != grid.CellsMask ? cellZ + 1 : cellZ;
for (int z = minCellZ; z <= maxCellZ; z++)
{
for (int y = minCellY; y <= maxCellY; y++)
{
for (int x = minCellX; x <= maxCellX; x++)
{
int cell = x | (y << grid.Cells2DLog) | (z << grid.Cells3DLog);
CollideChain(ref collider, in entity, _cellHeads[grid.CellOffset + cell]);
}
}
}
}
}
}
public void Query<TQuery>(float minX, float minY, float minZ, float maxX, float maxY, float maxZ, ref TQuery query)
where TQuery : struct, IHshgQuery3D
{
UpdateCache();
(int xStart, int xEnd) = MapPosition(minX, maxX);
(int yStart, int yEnd) = MapPosition(minY, maxY);
(int zStart, int zEnd) = MapPosition(minZ, maxZ);
int gridIndex = 0;
int shift = 0;
while (gridIndex < _grids.Length && _grids[gridIndex].EntitiesLen == 0)
{
gridIndex++;
shift++;
}
if (gridIndex == _grids.Length)
{
return;
}
xStart >>= shift;
yStart >>= shift;
zStart >>= shift;
xEnd >>= shift;
yEnd >>= shift;
zEnd >>= shift;
while (true)
{
ref Grid grid = ref _grids[gridIndex];
int sx = xStart != 0 ? xStart - 1 : 0;
int sy = yStart != 0 ? yStart - 1 : 0;
int sz = zStart != 0 ? zStart - 1 : 0;
int ex = xEnd != grid.CellsMask ? xEnd + 1 : xEnd;
int ey = yEnd != grid.CellsMask ? yEnd + 1 : yEnd;
int ez = zEnd != grid.CellsMask ? zEnd + 1 : zEnd;
for (int z = sz; z <= ez; z++)
{
for (int y = sy; y <= ey; y++)
{
for (int x = sx; x <= ex; x++)
{
int cell = x | (y << grid.Cells2DLog) | (z << grid.Cells3DLog);
for (int j = _cellHeads[grid.CellOffset + cell]; j != 0;)
{
ref HshgEntity3D entity = ref _entities[j];
if (entity.X + entity.Radius >= minX &&
entity.X - entity.Radius <= maxX &&
entity.Y + entity.Radius >= minY &&
entity.Y - entity.Radius <= maxY &&
entity.Z + entity.Radius >= minZ &&
entity.Z - entity.Radius <= maxZ)
{
query.Query(this, in entity);
}
j = entity.Next;
}
}
}
}
if (grid.Shift == 0)
{
break;
}
shift = grid.Shift;
xStart >>= shift;
yStart >>= shift;
zStart >>= shift;
xEnd >>= shift;
yEnd >>= shift;
zEnd >>= shift;
gridIndex += shift;
}
}
public void OptimizeAndRemapReferences<T>(T[] sourceReferences, T[] destinationReferences)
{
HshgEntity3D[] entities = _optimizeBuffer is { } buffer && buffer.Length >= _entities.Length
? buffer
: GC.AllocateUninitializedArray<HshgEntity3D>(_entities.Length);
HshgEntity3D[] oldEntities = _entities;
ref int cellHeads = ref MemoryMarshal.GetArrayDataReference(_cellHeads);
ref HshgEntity3D source = ref MemoryMarshal.GetArrayDataReference(oldEntities);
ref HshgEntity3D destination = ref MemoryMarshal.GetArrayDataReference(entities);
ref T destinationReference = ref MemoryMarshal.GetArrayDataReference(destinationReferences);
int index = 1;
int referenceIndex = 0;
for (int cell = 0; cell < _cellHeads.Length; cell++)
{
ref int cellHead = ref Unsafe.Add(ref cellHeads, cell);
int entityIndex = cellHead;
if (entityIndex == 0)
{
continue;
}
cellHead = index;
while (true)
{
ref HshgEntity3D entity = ref Unsafe.Add(ref destination, index);
entity = Unsafe.Add(ref source, entityIndex);
Unsafe.Add(ref destinationReference, referenceIndex) = sourceReferences[entity.Reference];
entity.Reference = referenceIndex++;
if (entity.Prev != 0)
{
entity.Prev = index - 1;
}
index++;
if (entity.Next != 0)
{
entityIndex = entity.Next;
entity.Next = index;
}
else
{
break;
}
}
}
_optimizeBuffer = oldEntities;
_entities = entities;
_entitiesUsed = index;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void CollideChain<TCollider>(ref TCollider collider, in HshgEntity3D entity, int head)
where TCollider : struct, IHshgCollider3D
{
while (head != 0)
{
ref HshgEntity3D other = ref _entities[head];
collider.Collide(this, in entity, in other);
head = other.Next;
}
}
private int GetEntityIndex()
{
if (_entitiesUsed == _entities.Length)
{
EnsureCapacity(_entities.Length << 1);
}
return _entitiesUsed++;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void Reinsert(int index)
{
ref HshgEntity3D entity = ref _entities[index];
ref Grid grid = ref _grids[entity.Grid];
entity.Cell = GetCell(in grid, entity.X, entity.Y, entity.Z);
int cellIndex = grid.CellOffset + entity.Cell;
entity.Next = _cellHeads[cellIndex];
if (entity.Next != 0)
{
_entities[entity.Next].Prev = index;
}
entity.Prev = 0;
_cellHeads[cellIndex] = index;
if (grid.EntitiesLen == 0)
{
_newCache |= 1u << entity.Grid;
}
grid.EntitiesLen++;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void RemoveLight(int index)
{
ref HshgEntity3D entity = ref _entities[index];
ref Grid grid = ref _grids[entity.Grid];
if (entity.Prev == 0)
{
_cellHeads[grid.CellOffset + entity.Cell] = entity.Next;
}
else
{
_entities[entity.Prev].Next = entity.Next;
}
_entities[entity.Next].Prev = entity.Prev;
grid.EntitiesLen--;
if (grid.EntitiesLen == 0)
{
_newCache ^= 1u << entity.Grid;
}
}
private void UpdateCache()
{
if (_oldCache == _newCache)
{
return;
}
_oldCache = _newCache;
for (int i = 0; i < _grids.Length; i++)
{
_grids[i].Shift = 0;
}
int oldGrid = 0;
while (oldGrid < _grids.Length && _grids[oldGrid].EntitiesLen == 0)
{
oldGrid++;
}
if (oldGrid == _grids.Length)
{
return;
}
byte shift = 1;
for (int newGrid = oldGrid + 1; newGrid < _grids.Length; newGrid++)
{
if (_grids[newGrid].EntitiesLen == 0)
{
shift++;
continue;
}
_grids[oldGrid].Shift = shift;
oldGrid = newGrid;
shift = 1;
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private byte GetGrid(float radius)
{
uint rounded = (uint)(radius + radius);
if (rounded < (uint)_cellSize)
{
return 0;
}
int grid = _cellLog - BitOperations.LeadingZeroCount(rounded) + 1;
return (byte)Math.Min(grid, _grids.Length - 1);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private static int GetCell1D(in Grid grid, float x)
{
int cell = (int)(MathF.Abs(x) * grid.InverseCellSize);
return (cell & grid.CellsSide) != 0
? grid.CellsMask - (cell & grid.CellsMask)
: cell & grid.CellsMask;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private static int GetCell(in Grid grid, float x, float y, float z) =>
GetCell1D(in grid, x) | (GetCell1D(in grid, y) << grid.Cells2DLog) | (GetCell1D(in grid, z) << grid.Cells3DLog);
private (int Start, int End) MapPosition(float x1Raw, float x2Raw)
{
float x1;
float x2;
if (x1Raw < 0)
{
float shift = ((((int)(-x1Raw * _inverseGridSize)) << 1) + 2) * _gridSize;
x1 = x1Raw + shift;
x2 = x2Raw + shift;
}
else
{
x1 = x1Raw;
x2 = x2Raw;
}
ref Grid grid = ref _grids[0];
int folds = (int)((x2 - ((int)(x1 * _inverseGridSize)) * _gridSize) * _inverseGridSize);
switch (folds)
{
case 0:
{
int cell = GetCell1D(in grid, x1);
int end = GetCell1D(in grid, x2);
return (Math.Min(cell, end), Math.Max(cell, end));
}
case 1:
{
int cell = (int)(MathF.Abs(x1) * grid.InverseCellSize);
int end = GetCell1D(in grid, x2);
return (cell & grid.CellsSide) != 0
? (0, Math.Max(grid.CellsMask - (cell & grid.CellsMask), end))
: (Math.Min(cell & grid.CellsMask, end), grid.CellsMask);
}
default:
return (0, grid.CellsMask);
}
}
private static bool IsPowerOfTwo(int value) => value > 0 && (value & (value - 1)) == 0;
private static int MaxGrids(int side)
{
int grids = 0;
do
{
grids++;
side >>= 1;
}
while (side >= 2);
return grids;
}
private static int MaxCells(int side)
{
long cells = 0;
do
{
cells += (long)side * side * side;
side >>= 1;
}
while (side >= 2);
return (int)cells;
}
private struct Grid
{
public int CellOffset;
public int CellsSide;
public int CellsSq;
public int CellsMask;
public int Cells2DLog;
public int Cells3DLog;
public byte Shift;
public float InverseCellSize;
public int EntitiesLen;
}
}
internal struct Ball2D
{
public float Vx;
public float Vy;
}
internal struct Ball3D
{
public float Vx;
public float Vy;
public float Vz;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment