Penetration/Push based collision response algorithms