Correct penetration vector calculations

Fix unit tests