Fix collision code

Fix problem colliding when entities are contained in each other.