Add ability to calculate penetration vector