Updated to simpler contact structure
Fixed bugs with new contact mechanism