Took updated math from Manbil
Fixed up world a bit
More work on GJK and raycast improvement