An Energy Based Corner to Contact Algorithm