Refactored and tested the physics further. I'm not 100% happy with the current situation but it almost works.