Contact Problem

Very Similar question to How to implement the penalty method to avoid self-contact - #2 by dokken