Some bugfixes to GJK