Add one-shot contact manifold generator.
Improved Johnson subalgorithm.

Some code was moved around and it uses unsafe indexing.
This improves johnson simplex updates by 40%.