public static void ResolveAll(List <IRigidBody> bodies, List <IRigidBody> mapbodies, QuadTree <IRigidBody> QTbodies, QuadTree <IRigidBody> QTmap)
{
var collisionData = new ConcurrentBag <CollisionData>();
//foreach (var b in bodies)
bodies.AsParallel().ForAll(b =>
{
var potHits = QTbodies.Query(b.BoundingBox); //broad-phase
potHits.Remove(b); //the body itself will be included in query results.
potHits.AddRange(QTmap.Query(b.BoundingBox));
if (potHits.Count > 0)
{
var allCollData = TestAndGetAllCollisionData(b, potHits); //narrow-phase
if (allCollData.Count > 0)
{
CollisionData aggrColData = AggrCollisionData(allCollData);
collisionData.Add(aggrColData);
}
}
});
foreach (var cd in collisionData)
{
ApplyCollisionData(cd); //apply all aggregated collision responses.
}
}
public void Update(float dt)
{
deleteQueued();
updateQTs();
foreach (var b in ActiveBodies)
{
//reset forces + torques...
b.FNET = Vector2.Zero;
b.TNET = 0.0f;
//add forces + torques...
foreach (var uf in uforces)
{
b.FNET += uf.GetForce(b);
}
foreach (var ut in utorques)
{
b.TNET += ut.GetTorque(b);
}
}
bffields.ForEach(bff =>
{
var pots = QTbodies.Query(bff.AreaInfluenced);
pots.Remove(bff.SourceBody);
pots.ForEach(b =>
{
var f = bff.GetForce(b.Position - bff.SourceBody.Position);
b.FNET += f;
bff.SourceBody.FNET += -f; //newton's third law force
});
}
);
iffields.ForEach(iff =>
{
var pots = QTbodies.Query(iff.AreaInfluenced);
pots.ForEach(b =>
{
var f = iff.GetForce(b.Position - iff.SourcePos);
b.FNET += f;
});
}
);
iffields.Clear();
iforces.ForEach(iforce => iforce.TargetBody.FNET += iforce.Force);
iforces.Clear();
foreach (var b in ActiveBodies)
{
var a = b.FNET * b.InvMass();
b.Velocity += a * dt;
b.Position += b.Velocity * dt;
b.Rotation += b.AngularVelocity * dt;
}
CollisionEngine.ResolveAll(ActiveBodies, MapBodies, QTbodies, QTmap);
int numXtra = 1;
for (int i = 0; i < numXtra; i++) //extra rounds of collision resolution.
{
updateQTs();
CollisionEngine.ResolveAll(ActiveBodies, MapBodies, QTbodies, QTmap);
}
//handle particle systems...
updateParticleSys(dt);
}