Fixed issue where the wrong axis was being used to test for separation.
Added some unit testing for SAT.