private void DistributeCircle_Box(LSBody box, LSBody circle)
{
xMore = circle._position.x > box._position.x;
yMore = circle._position.y > box._position.y;
if (xMore)
{
PenetrationX = (circle.XMin - box.XMax);
}
else
{
PenetrationX = (circle.XMax - box.XMin);
}
if (yMore)
{
PenetrationY = (circle.YMin - box.YMax);
}
else
{
PenetrationY = (circle.YMax - box.YMin);
}
//PenetrationX = PenetrationX + circle.Velocity.x;
//PenetrationY = PenetrationY + circle.Velocity.y;
xAbs = PenetrationX < 0 ? -PenetrationX : PenetrationX;
yAbs = PenetrationY < 0 ? -PenetrationY : PenetrationY;
if ((xAbs <= circle.Radius && yAbs <= circle.Radius))
{
Vector2d corner;
corner.x = xMore ? box.Position.x + box.HalfWidth : box.Position.x - box.HalfWidth;
corner.y = yMore ? box.Position.y + box.HalfHeight : box.Position.y - box.HalfHeight;
Vector2d dir = circle.Position - corner;
dir.Normalize();
circle.Position = corner + dir * circle.Radius;
}
else
{
if (xAbs > yAbs)
{
PenetrationX = 0;
//if (yAbs < circle.Radius) PenetrationY = PenetrationY * yAbs / circle.Radius;
if (PenetrationY > 0 == yMore)
{
PenetrationY = -PenetrationY;
}
}
else
{
PenetrationY = 0;
//if (xAbs < circle.Radius) PenetrationX = PenetrationX * xAbs / circle.Radius;
if (PenetrationX > 0 == xMore)
{
PenetrationX = -PenetrationX;
}
}
//Resolving
circle._position.x -= PenetrationX;
circle._position.y -= PenetrationY;
}
circle.PositionChanged = true;
circle.BuildBounds();
}
