Collision detection fix

Was calling objects.erase(iter) while iter == objects.end(), this is an invalid iterator for erasure and was causing an exception to be thrown during the collision check, leaving objects in an unknown state of awareness.

Further simplified the logic of this function.