public static void BuildChunks(SAPChunk chunk)
{
int length = chunk.Length;
int freeIndex = chunk.FreeIndex;
if (freeIndex >= length)
{
return;
}
BroadphaseAABB[] items = chunk.Items;
int current = freeIndex + 1;
while (current < length)
{
while (current < length && items[current].Id == uint.MaxValue)
{
current++;
}
if (current < length)
{
items[freeIndex++] = items[current++];
}
}
chunk.Length = freeIndex;
chunk.FreeIndex = int.MaxValue;
}
private unsafe void CalculatePairs(SAPChunk chunk)
{
chunk.PairLength = 0;
int length = chunk.Length;
_comparer.UpdateSortAxis(chunk.SortAxis);
Array.Sort(chunk.Items, 0, length, _comparer);
float2 s = float2.zero;
float2 s2 = float2.zero;
for (int i = 0; i < length; i++)
{
BroadphaseAABB a = chunk.Items[i];
float2 p = (a.AABB->Min + a.AABB->Max) * 0.5f;
s += p;
s2 += p * p;
for (int j = i + 1; j < length; j++)
{
BroadphaseAABB b = chunk.Items[j];
if (b.AABB->Min[chunk.SortAxis] > a.AABB->Max[chunk.SortAxis])
{
break;
}
if (a.IsStatic && b.IsStatic)
{
continue;
}
if (!a.AABB->Overlap(*b.AABB))
{
continue;
}
if (!_collisionMatrix.Check(a.Layer, b.Layer))
{
continue;
}
if (chunk.PairLength >= chunk.Pairs.Length)
{
Array.Resize(ref chunk.Pairs, 2 * chunk.PairLength);
}
chunk.Pairs[chunk.PairLength++] = new BroadphasePair(a.Entity, b.Entity);
}
}
float2 v = s2 / length - s * s / (length * length);
chunk.SortAxis = v[1] > v[0] ? 1 : 0;
}
public static SAPChunk GetOrCreateChunk(int chunkId, BroadphaseSAPComponent bpChunks)
{
if (bpChunks.Chunks.TryGetValue(chunkId, out SAPChunk bpChunk))
{
return(bpChunk);
}
bpChunk = new SAPChunk(chunkId);
bpChunks.Chunks.Add(chunkId, bpChunk);
return(bpChunk);
}
public static void RemoveFormChunk(SAPChunk chunk, uint entityId)
{
int index = Array.FindIndex(chunk.Items, 0, chunk.Length, bp => bp.Id == entityId);
if (index < 0)
{
throw new InvalidOperationException($"entity by id '{entityId}' not found in chunk '{chunk.Id}'");
}
BroadphaseAABB item = chunk.Items[index];
if (!item.IsStatic)
{
chunk.DynamicCounter--;
}
item.Id = uint.MaxValue;
item.Entity = null;
chunk.Items[index] = item;
chunk.FreeIndex = math.min(chunk.FreeIndex, index);
}
public unsafe void Update(float deltaTime, EcsWorld world)
{
BroadphaseSAPComponent bpChunks = world.GetOrCreateSingleton <BroadphaseSAPComponent>();
world.Filter(_entitiesFilter).ForEach(
(IEcsEntity entity, TransformComponent tr, ColliderComponent col, BroadphaseRefComponent bpRef) =>
{
AABB aabb = new AABB(col.Size, tr.Position,
col.ColliderType == ColliderType.Rect ? tr.Rotation : 0f);
fixed(AABB * pAABB = &bpRef.AABB)
{
pAABB->Min = aabb.Min;
pAABB->Max = aabb.Max;
}
int chunksHash = BroadphaseHelper.CalculateChunksHash(aabb);
if (bpRef.ChunksHash == chunksHash)
{
return;
}
RigBodyComponent rig = entity.GetComponent <RigBodyComponent>();
uint entityId = entity.Id;
bool isStatic = MathHelper.Equal(rig.InvMass, 0);
int layer = col.Layer;
List <SAPChunk> chunks = bpRef.Chunks;
List <SAPChunk> newChunks = new List <SAPChunk>(4);
foreach (int chunkId in BroadphaseHelper.GetChunks(aabb))
{
int index = -1;
for (int i = 0; i < chunks.Count; i++)
{
SAPChunk chunk = chunks[i];
if (chunk == null || chunk.Id != chunkId)
{
continue;
}
index = i;
break;
}
if (index >= 0)
{
SAPChunk chunk = chunks[index];
chunks[index] = null;
newChunks.Add(chunk);
}
else
{
SAPChunk chunk = BroadphaseHelper.GetOrCreateChunk(chunkId, bpChunks);
if (chunk.Length >= chunk.Items.Length)
{
Array.Resize(ref chunk.Items, 2 * chunk.Length);
}
fixed(AABB * pAABB = &bpRef.AABB)
{
chunk.Items[chunk.Length++] = new BroadphaseAABB
{
AABB = pAABB,
Id = entityId,
IsStatic = isStatic,
Layer = layer,
Entity = entity
};
}
if (!isStatic)
{
chunk.DynamicCounter++;
}
newChunks.Add(chunk);
}
}
foreach (SAPChunk chunk in chunks)
{
if (chunk == null)
{
continue;
}
BroadphaseHelper.RemoveFormChunk(chunk, entityId);
}
bpRef.Chunks = newChunks;
bpRef.ChunksHash = chunksHash;
});
}
public unsafe void Update(float deltaTime, EcsWorld world)
{
BroadphaseSAPComponent bpChunks = world.GetOrCreateSingleton <BroadphaseSAPComponent>();
world.Filter(_rayFilter).ForEach((IEcsEntity entity, TransformComponent tr, RayComponent ray) =>
{
ray.Hit = false;
ray.Source = tr.Position;
ray.Rotation = tr.Rotation;
float minDist = float.MaxValue;
RayTrace(ray, ref _chunksBuffer, ref _pointsBuffer, out int length);
for (int i = 0; i < length - 1; i++)
{
SAPChunk chunk = BroadphaseHelper.GetOrCreateChunk(_chunksBuffer[i], bpChunks);
float2 p1 = _pointsBuffer[i];
float2 p2 = _pointsBuffer[i + 1];
AABB sAABB = new AABB
{
Min = new float2(math.min(p1.x, p2.x), math.min(p1.y, p2.y)),
Max = new float2(math.max(p1.x, p2.x), math.max(p1.y, p2.y))
};
for (int j = 0; j < chunk.Length; j++)
{
BroadphaseAABB item = chunk.Items[j];
if (!item.AABB->Overlap(sAABB))
{
continue;
}
if (!_collisionMatrix.Check(ray.Layer, item.Layer))
{
continue;
}
IEcsEntity targetEntity = item.Entity;
if (entity == targetEntity)
{
continue;
}
tr = targetEntity.GetComponent <TransformComponent>();
ColliderComponent col = targetEntity.GetComponent <ColliderComponent>();
float2 hitPoint = float2.zero;
if (col.ColliderType == ColliderType.Circle && !OnCircleIntersection(ray, col, tr, out hitPoint) ||
col.ColliderType == ColliderType.Rect && !OnRectIntersection(ray, col, tr, out hitPoint))
{
continue;
}
float dist = math.distancesq(p1, hitPoint);
if (!(dist < minDist))
{
continue;
}
minDist = dist;
ray.HitPoint = hitPoint;
}
if (!(minDist < float.MaxValue))
{
continue;
}
ray.Hit = true;
break;
}
});
}
