Move body to body collision resolution to separate function and some minor refactoring