WIP: Collision respoonse works with SOFA's penalty method.

Issue #3
Issue #4