Improve collision perfomance a lot. Various Cleanup and bugfixes.