Any contact type/number refactoring

Adapt the force torque preint related code so that any number of contact can be included and any type (3D point foot, 6D humanoid foot).