Implemented collision resolution and fixed some bug in the manifold
generator which was due to wrong positions being used.