Add capsule check to fix collision penetration for real this time