Add penetration compensation

Compensate floating point errors by setting the position of overlapping
objects apart a bit