WIP: Fix collision detection bugs, debug for collision response

Issue #3
Issue #4