More improvments and bug fixes to collision detection