public void Add(Body2D body)
{
if (body.IsStatic)
{
staticBodies.Add(body);
}
else
{
dynamicBodies.Add(body);
}
}
public PhysicsTester()
{
Body2D staticBox = new Body2D(new Rectangle(300, 75), null);
staticBox.Position = new vec2(400, 300);
staticBox.IsStatic = true;
Body2D dynamicBox = new Body2D(new Capsule(60, 20), null);
dynamicBox.Position = new vec2(400, 150);
world = new World2D();
world.Add(staticBox);
world.Add(dynamicBox);
MessageSystem.Subscribe(this, CoreMessageTypes.Keyboard, (messageType, data, dt) =>
{
ProcessKeyboard((KeyboardData)data);
});
}
private void ComputeManifold(Body2D body)
{
List <vec2> list = new List <vec2>();
// It's assumed that all dynamic bodies in this context will use capsules for character control. The
// capsule will also always remain upright (i.e. rotation is fixed).
Capsule capsule = (Capsule)body.Shape;
foreach (var staticBody in staticBodies)
{
Rectangle rect = (Rectangle)staticBody.Shape;
vec2 p1 = capsule.Position;
vec2 p2 = rect.Position;
float dX = Math.Abs(p1.x - p2.x);
float dY = Math.Abs(p1.y - p2.y);
float r = capsule.Radius;
float sumX = rect.Width / 2 + capsule.Radius;
float sumY = rect.Height / 2 + capsule.Height / 2 + r;
bool overlapsX = dX <= sumX;
bool overlapsY = dY <= sumY;
if (!(overlapsX && overlapsY))
{
continue;
}
// This means the capsule must be colliding from the top or bottom.
if (dX <= rect.Width / 2)
{
float correction = sumY - dY;
if (p1.y > p2.y)
{
correction *= -1;
}
capsule.Y -= correction;
return;
}
// This means the capsule must be colliding from the left or right.
if (dY <= (rect.Height + capsule.Height) / 2)
{
float correction = sumX - dX;
if (p1.x > p2.x)
{
correction *= -1;
}
capsule.X -= correction;
return;
}
bool left = p1.x < p2.x;
bool above = p1.y < p2.y;
float x = left ? rect.Left : rect.Right;
float y = above ? rect.Top : rect.Bottom;
vec2 corner = new vec2(x, y);
vec2 center = p1 + new vec2(0, capsule.Height / 2 * (above ? 1 : -1));
float squared = Utilities.DistanceSquared(corner, center);
if (squared >= r * r)
{
continue;
}
float distance = (float)Math.Sqrt(squared);
float delta = r - distance;
capsule.Position += Utilities.Normalize(center - corner) * delta;
}
//return ComputeFinalCorrection(list);
}