remove redundant test
reduce logging in cleanup in VERBOSE
split up some large functions in rigid bodies