based on Advanced Character Physics by Thomas Jakobsen