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