Issues with Coriolis MatrixAlgorithm when floating base is not the root link

[LoreMoretti]

The Coriolis Matrix Algorithm introduced by #1283 does not work fine when:

1. The floating base is not the link with LinkIndex = 0
2. The floating base is an additional frame of a link and not the main frame of the link itself.

This commit c2d939a adds a unit test to reproduce these issues.