Collision test is now working. AH-290

All that is left is to calculate overlap depth.
Added some unit tests. AH-290.