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 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(_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;
}
});
}
