Various collision detection fixes and optimizations