bullet-featherstone: fix SetWorldPose with off-diagonal moment of inertia

[iche033]

🦟 Bug fix

Summary

For a body that has non-zero off-diagonal elements of moment of inertia, setting its world pose gave unexpected results:

This is because now there is a rotational offset in its principal axes. This PR fixes setting world pose by taking into account the rotational offset.

Checklist

• [x] Signed all commits for DCO
• [x] Added tests
• [ ] Updated documentation (as needed)
• [ ] Updated migration guide (as needed)
• [ ] Consider updating Python bindings (if the library has them)
• [ ] codecheck passed (See contributing)
• [ ] All tests passed (See test coverage)
• [ ] While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.