Testing a single contact per manifold
Checking contact lengths before insertion into the vector
Improved force application