[algorithm] Enable the use of the algorithm in a generic way to handle cases with multiple collision surfaces

[th-skam]

Should be able to use the solver ID to access the appropriate constraint forces