Resolve collisions in physics engine

Now that the penetration vector is working better, take that into
account and move the necessary objects so that they aren't colliding.