public void Update(float deltaTime, EcsWorld world)
{
BroadphaseSAPComponent bpChunks =
world.GetOrCreateSingleton <BroadphaseSAPComponent>();
foreach (BroadphasePair pair in bpChunks.Pairs)
{
IEcsEntity entityA = pair.EntityA;
IEcsEntity entityB = pair.EntityB;
TransformComponent ta = entityA.GetComponent <TransformComponent>();
TransformComponent tb = entityB.GetComponent <TransformComponent>();
ColliderComponent ca = entityA.GetComponent <ColliderComponent>();
ColliderComponent cb = entityB.GetComponent <ColliderComponent>();
ContactInfo info;
if (ca.ColliderType == ColliderType.Circle && cb.ColliderType == ColliderType.Circle)
{
OnCircleCircleCollision(ca, ta, cb, tb, out info);
}
else if (ca.ColliderType == ColliderType.Circle && cb.ColliderType == ColliderType.Rect)
{
OnCircleRectCollision(ca, ta, cb, tb, out info);
}
else if (ca.ColliderType == ColliderType.Rect && cb.ColliderType == ColliderType.Circle)
{
OnCircleRectCollision(cb, tb, ca, ta, out info);
info.Normal = -info.Normal;
}
else
{
throw new InvalidOperationException();
}
if (!info.Hit)
{
continue;
}
RigBodyComponent ra = entityA.GetComponent <RigBodyComponent>();
RigBodyComponent rb = entityB.GetComponent <RigBodyComponent>();
float2 rv = rb.Velocity - ra.Velocity;
float contactVel = math.dot(rv, info.Normal);
float invMassSum = ra.InvMass + rb.InvMass;
float f = -contactVel / invMassSum;
float2 impulse = info.Normal * f * deltaTime;
ra.Velocity -= ra.InvMass * impulse;
rb.Velocity += rb.InvMass * impulse;
float2 correction = info.Penetration / (ra.InvMass + rb.InvMass) * info.Normal * 0.5f;
ta.Position -= correction * ra.InvMass;
tb.Position += correction * rb.InvMass;
}
}
public unsafe void Update(float deltaTime, EcsWorld world)
{
BroadphaseSAPComponent bpChunks = world.GetOrCreateSingleton <BroadphaseSAPComponent>();
IEcsEntity[] entities = world.Filter(_entitiesFilter).ToEntityArray();
for (int i = 0; i < entities.Length; i++)
{
IEcsEntity entity = entities[i];
uint entityId = entity.Id;
TransformComponent tr = entity.GetComponent <TransformComponent>();
ColliderComponent col = entity.GetComponent <ColliderComponent>();
RigBodyComponent rig = entity.GetComponent <RigBodyComponent>();
AABB aabb = new AABB(col.Size, tr.Position, col.ColliderType == ColliderType.Rect ? tr.Rotation : 0f);
bool isStatic = MathHelper.Equal(rig.InvMass, 0);
int layer = col.Layer;
List <SAPChunk> chunks = new List <SAPChunk>(4);
foreach (int chunkId in BroadphaseHelper.GetChunks(aabb))
{
chunks.Add(BroadphaseHelper.GetOrCreateChunk(chunkId, bpChunks));
}
BroadphaseRefComponent bpRef = new BroadphaseRefComponent
{
Chunks = chunks,
ChunksHash = BroadphaseHelper.CalculateChunksHash(aabb),
AABB = aabb
};
entity.AddComponent(bpRef);
foreach (SAPChunk chunk in chunks)
{
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++; }
}
}
}
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;
});
}
