fixed collision detection

* better normal estimation
* some minor glitches left
* cleaned up test code