commit GJK code seems to be working pretty well