[C++][unittest] Fix computations in jointcomposite::calc() + reworked unittest (comparison of R3xSO3 (either two joints with no inertia between them or Jointcomposite))

[C++] Make JointComposite part of the JointVariant. Reoganized some include to avoid circular dependencies