Implement interpenetration resolution